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 31 commits into
base: main
Choose a base branch
from
Open
Changes from all commits
Commits
Show all changes
31 commits
Select commit Hold shift + click to select a range
1fa5a8a
add implicit actuator init and effort limit test
jtigue-bdai Apr 8, 2025
3b9d9f7
add edge cases and velocity limit cfg tests
jtigue-bdai Apr 8, 2025
97ea7f0
rename to test_implicit_actuator
jtigue-bdai Apr 9, 2025
c1dab1e
cleanup and add ideal_pd tests
jtigue-bdai Apr 9, 2025
cddbeb5
add ideal_pd compute test
jtigue-bdai Apr 9, 2025
283ef5c
dc motor clip test for positive power
jtigue-bdai Apr 10, 2025
ca7b1a0
change log
jtigue-bdai Apr 10, 2025
c6e757f
fix test and clipping
jtigue-bdai Apr 11, 2025
37674e8
update documentation
jtigue-bdai Apr 11, 2025
ba0e4e1
update changelog
jtigue-bdai Apr 11, 2025
3e50893
change axes of ascii torque-speed curve
jtigue-bdai Apr 14, 2025
a96e1d6
sepereate dc motor tests to anothe file
jtigue-bdai May 9, 2025
b6138ae
update dc motor docs
jtigue-bdai May 9, 2025
ee8c3bb
fix test docstring
jtigue-bdai May 9, 2025
38db7a0
Merge branch 'main' into jat/feat/q4_dc_motor_limits
kellyguo11 May 13, 2025
5cce90e
Merge branch 'main' into jat/feat/q4_dc_motor_limits
jtigue-bdai May 14, 2025
91863cb
Merge branch 'main' into jat/feat/q4_dc_motor_limits
kellyguo11 May 15, 2025
03649aa
fix test convertion to pytest
jtigue-bdai May 16, 2025
a62d818
Merge branch 'main' into jat/feat/q4_dc_motor_limits
jtigue-bdai May 16, 2025
4359f0b
fix format
jtigue-bdai May 16, 2025
0299af9
Trigger build
jtigue-bdai May 16, 2025
96f2365
Trigger build
jtigue-bdai May 16, 2025
7cf4b59
Merge branch 'main' into jat/feat/q4_dc_motor_limits
jtigue-bdai May 20, 2025
c104e38
fix clippling for very large input velocities
jtigue-bdai May 19, 2025
e42f04f
fix format
jtigue-bdai May 20, 2025
14e4df3
reduce numerical clamping
jtigue-bdai May 20, 2025
477d25d
Merge branch 'main' into jat/feat/q4_dc_motor_limits
kellyguo11 May 24, 2025
9816be1
Update extension.toml
kellyguo11 May 24, 2025
58740b8
Merge branch 'main' into jat/feat/q4_dc_motor_limits
kellyguo11 May 26, 2025
d207856
add clamp fix to lstm net actuator
jtigue-bdai May 27, 2025
43f7f44
Merge branch 'main' into jat/feat/q4_dc_motor_limits
kellyguo11 May 29, 2025
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
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
@@ -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:
3 changes: 1 addition & 2 deletions source/isaaclab/config/extension.toml
Original file line number Diff line number Diff line change
@@ -1,8 +1,7 @@
[package]

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

version = "0.40.1"

# Description
title = "Isaac Lab framework for Robot Learning"
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.40.1 (2025-05-20)
~~~~~~~~~~~~~~~~~~~

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.40.0 (2025-05-16)
~~~~~~~~~~~~~~~~~~~

6 changes: 4 additions & 2 deletions source/isaaclab/isaaclab/actuators/actuator_net.py
Original file line number Diff line number Diff line change
@@ -79,7 +79,7 @@ def compute(
# compute network inputs
self.sea_input[:, 0, 0] = (control_action.joint_positions - joint_pos).flatten()
self.sea_input[:, 0, 1] = joint_vel.flatten()
# save current joint vel for dc-motor clipping
# save current joint vel for dc-motor clipping and clip input speed significantly above the max velocity
self._joint_vel[:] = joint_vel

# run network inference
@@ -90,7 +90,9 @@ def compute(
self.computed_effort = torques.reshape(self._num_envs, self.num_joints)

# clip the computed effort based on the motor limits
self.applied_effort = self._clip_effort(self.computed_effort)
self.applied_effort = torch.clip(
self._clip_effort(self.computed_effort), min=-self.effort_limit, max=self.effort_limit
)

# return torques
control_action.joint_efforts = self.applied_effort
68 changes: 46 additions & 22 deletions source/isaaclab/isaaclab/actuators/actuator_pd.py
Original file line number Diff line number Diff line change
@@ -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.
@@ -211,23 +211,28 @@ 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, stall}`): 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`).
* Corner velocity (:math:`V_{c}`): The velocity where the torque-speed curve intersects with continuous torque.

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, stall} \times \left(1 -
\frac{\dot{q}}{\dot{q}_{j, max}}\right), -∞, \tau_{j, con} \right) \\
\tau_{j, min}(\dot{q}) & = clip \left (\tau_{j, stall} \times \left( -1 -
\frac{\dot{q}}{\dot{q}_{j, max}}\right), - \tau_{j, con}, ∞ \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} =
\gamma \times \tau_{motor, max}` and :math:`\tau_{j, peak} = \gamma \times \tau_{motor, peak}`
are the maximum joint velocity, maximum joint torque and peak torque, respectively. These parameters
:math:`\dot{q}_{j, max} = \gamma^{-1} \times \dot{q}_{motor, max}`, :math:`\tau_{j, con} =
\gamma \times \tau_{motor, con}` and :math:`\tau_{j, stall} = \gamma \times \tau_{motor, stall}`
are the maximum joint velocity, continuous joint torque and stall torque, respectively. These parameters
are read from the configuration instance passed to the class.

Using these values, the computed torques are clipped to the minimum and maximum values based on the
@@ -237,6 +242,18 @@ class DCMotor(IdealPDActuator):

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

If the velocity of the joint is outside corner velocities (this would be due to external forces) the
applied output torque will be driven to the torque speed curve providing braking torque that will try to drive the
motor down to the corner velocities

.. math::

if \quad & \dot{q}>V_{c} \quad & then \quad & \tau_{j, applied} = \tau_{j, stall} \times \left(1 -
\frac{\dot{q}}{\dot{q}_{j, max}}\right) \\
if \quad & \dot{q}<-V_{c} \quad & then \quad & \tau_{j, applied} = \tau_{j, stall} \times \left(-1 -
\frac{\dot{q}}{\dot{q}_{j, max}}\right)

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

cfg: DCMotorCfg
@@ -245,10 +262,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 in the second and fourth quadrant
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
@@ -275,15 +293,21 @@ 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)
gt_vel_at_effort_lim = self._joint_vel > self._vel_at_effort_lim
lt_vel_at_neg_effort_lim = self._joint_vel < -self._vel_at_effort_lim
clamped[gt_vel_at_effort_lim] = torque_speed_top[gt_vel_at_effort_lim]
clamped[lt_vel_at_neg_effort_lim] = torque_speed_bottom[lt_vel_at_neg_effort_lim]

return clamped


class DelayedPDActuator(IdealPDActuator):
192 changes: 192 additions & 0 deletions source/isaaclab/test/actuators/test_dc_motor.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,192 @@
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

from isaaclab.app import AppLauncher

HEADLESS = True

# if not AppLauncher.instance():
simulation_app = AppLauncher(headless=HEADLESS).app

"""Rest of imports follows"""

import torch

import pytest

from isaaclab.actuators import DCMotorCfg


@pytest.mark.parametrize("num_envs", [1, 2])
@pytest.mark.parametrize("num_joints", [1, 2])
@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
def test_dc_motor_init_minimum(num_envs, num_joints, device):
joint_names = [f"joint_{d}" for d in range(num_joints)]
joint_ids = [d for d in range(num_joints)]
stiffness = 200
damping = 10
effort_limit = 60.0
saturation_effort = 100.0
velocity_limit = 50

actuator_cfg = DCMotorCfg(
joint_names_expr=joint_names,
stiffness=stiffness,
damping=damping,
effort_limit=effort_limit,
saturation_effort=saturation_effort,
velocity_limit=velocity_limit,
)
# assume Articulation class:
# - finds joints (names and ids) associate with the provided joint_names_expr

actuator = actuator_cfg.class_type(
actuator_cfg,
joint_names=joint_names,
joint_ids=joint_ids,
num_envs=num_envs,
device=device,
)

# check device and shape
torch.testing.assert_close(actuator.computed_effort, torch.zeros(num_envs, num_joints, device=device))
torch.testing.assert_close(actuator.applied_effort, torch.zeros(num_envs, num_joints, device=device))
torch.testing.assert_close(
actuator.effort_limit,
effort_limit * torch.ones(num_envs, num_joints, device=device),
)
torch.testing.assert_close(
actuator.velocity_limit, velocity_limit * torch.ones(num_envs, num_joints, device=device)
)


@pytest.mark.parametrize("num_envs", [1, 2])
@pytest.mark.parametrize("num_joints", [1, 2])
@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
@pytest.mark.parametrize("test_point", range(18))
def test_dc_motor_clip(num_envs, num_joints, device, test_point):
r"""Test the computation of the dc motor actuator 4 quadrant torque speed curve.
torque_speed_pairs of interest:

0 - fully inside torque speed curve and effort limit (quadrant 1)
1 - greater than effort limit but under torque-speed curve (quadrant 1)
2 - greater than effort limit and outside torque-speed curve (quadrant 1)
3 - less than effort limit but outside torque speed curve (quadrant 1)
4 - less than effort limit but outside torque speed curve (quadrant 2)
5 - fully inside torque speed curve and effort limit (quadrant 2)
6 - fully outside torque speed curve and -effort limit (quadrant 2)
7 - fully inside torque speed curve, outside -effort limit, and inside corner velocity (quadrant 2)
8 - fully inside torque speed curves, outside -effort limit, and outside corner velocity (quadrant2)
e - effort_limit
s - saturation_effort
v - velocity_limit
c - corner velocity
\ - torque-speed linear boundary between v and s
each torque_speed_point will be tested in quadrant 3 and 4
===========================================================
Torque
\ (+)
\ |
Q2 s Q1
| \ 2
\ | 1 \
c ---------------------e-----\
\ | \
\ | 0 \ 3
\ | \
(-)-----------v -------------o-------------v --------------(+) Speed
\ | \ 4
\ | 5 \
\ | \
\ -----e---------------------c
\ | \ 6
Q3 \ | 7 Q4 \
\s \
|\ 8 \
(-) \
============================================================
"""
effort_lim = 60
saturation_effort = 100.0
velocity_limit = 50

torque_speed_pairs = [
(30.0, 10.0), # 0
(70.0, 10.0), # 1
(80.0, 40.0), # 2
(30.0, 40.0), # 3
(-20.0, 90.0), # 4
(-30.0, 10.0), # 5
(-80.0, 110.0), # 6
(-80.0, 50.0), # 7
(-120.0, 90.0), # 8
(-30.0, -10.0), # -0
(-70.0, -10.0), # -1
(-80.0, -40.0), # -2
(-30.0, -40.0), # -3
(20.0, -90.0), # -4
(30.0, -10.0), # -5
(80.0, -110.0), # -6
(80.0, -50.0), # -7
(120.0, -90.0), # -8
]
expected_clipped_effort = [
30.0,
60.0,
20.0,
20.0,
-80.0,
-30.0,
-120.0,
-60.0,
-80.0,
-30.0,
-60.0,
-20,
-20,
80.0,
30.0,
120.0,
60.0,
80.0,
]

joint_names = [f"joint_{d}" for d in range(num_joints)]
joint_ids = [d for d in range(num_joints)]
stiffness = 200
damping = 10
effort_lim = 60
saturation_effort = 100.0
velocity_limit = 50
actuator_cfg = DCMotorCfg(
joint_names_expr=joint_names,
stiffness=stiffness,
damping=damping,
effort_limit=effort_lim,
velocity_limit=velocity_limit,
saturation_effort=saturation_effort,
)

actuator = actuator_cfg.class_type(
actuator_cfg,
joint_names=joint_names,
joint_ids=joint_ids,
num_envs=num_envs,
device=device,
stiffness=actuator_cfg.stiffness,
damping=actuator_cfg.damping,
)

ts = torque_speed_pairs[test_point]
torque = ts[0]
speed = ts[1]
actuator._joint_vel[:] = speed * torch.ones(num_envs, num_joints, device=device)
effort = torque * torch.ones(num_envs, num_joints, device=device)
clipped_effort = actuator._clip_effort(effort)

torch.testing.assert_close(
expected_clipped_effort[test_point] * torch.ones(num_envs, num_joints, device=device),
clipped_effort,
)
274 changes: 274 additions & 0 deletions source/isaaclab/test/actuators/test_ideal_pd_actuator.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,274 @@
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

from isaaclab.app import AppLauncher

HEADLESS = True

# if not AppLauncher.instance():
simulation_app = AppLauncher(headless=HEADLESS).app

"""Rest of imports follows"""

import torch

import pytest

from isaaclab.actuators import IdealPDActuatorCfg
from isaaclab.utils.types import ArticulationActions


@pytest.mark.parametrize("num_envs", [1, 2])
@pytest.mark.parametrize("num_joints", [1, 2])
@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
@pytest.mark.parametrize("usd_default", [False, True])
def test_ideal_pd_actuator_init_minimum(num_envs, num_joints, device, usd_default):
"""Test initialization of ideal pd actuator with minimum configuration."""

joint_names = [f"joint_{d}" for d in range(num_joints)]
joint_ids = [d for d in range(num_joints)]
stiffness = None if usd_default else 200
damping = None if usd_default else 10
friction = None if usd_default else 0.1
armature = None if usd_default else 0.2

actuator_cfg = IdealPDActuatorCfg(
joint_names_expr=joint_names,
stiffness=stiffness,
damping=damping,
armature=armature,
friction=friction,
)
# assume Articulation class:
# - finds joints (names and ids) associate with the provided joint_names_expr

# faux usd defaults
stiffness_default = 300
damping_default = 20
friction_default = 0.0
armature_default = 0.0

actuator = actuator_cfg.class_type(
actuator_cfg,
joint_names=joint_names,
joint_ids=joint_ids,
num_envs=num_envs,
device=device,
stiffness=stiffness_default,
damping=damping_default,
friction=friction_default,
armature=armature_default,
)

# check initialized actuator
assert actuator.is_implicit_model is False
# check device and shape
torch.testing.assert_close(actuator.computed_effort, torch.zeros(num_envs, num_joints, device=device))
torch.testing.assert_close(actuator.applied_effort, torch.zeros(num_envs, num_joints, device=device))

torch.testing.assert_close(
actuator.effort_limit, actuator._DEFAULT_MAX_EFFORT_SIM * torch.ones(num_envs, num_joints, device=device)
)
torch.testing.assert_close(
actuator.effort_limit_sim, actuator._DEFAULT_MAX_EFFORT_SIM * torch.ones(num_envs, num_joints, device=device)
)
torch.testing.assert_close(actuator.velocity_limit, torch.inf * torch.ones(num_envs, num_joints, device=device))
torch.testing.assert_close(actuator.velocity_limit_sim, torch.inf * torch.ones(num_envs, num_joints, device=device))

if not usd_default:
torch.testing.assert_close(actuator.stiffness, stiffness * torch.ones(num_envs, num_joints, device=device))
torch.testing.assert_close(actuator.damping, damping * torch.ones(num_envs, num_joints, device=device))
torch.testing.assert_close(actuator.armature, armature * torch.ones(num_envs, num_joints, device=device))
torch.testing.assert_close(actuator.friction, friction * torch.ones(num_envs, num_joints, device=device))
else:
torch.testing.assert_close(
actuator.stiffness, stiffness_default * torch.ones(num_envs, num_joints, device=device)
)
torch.testing.assert_close(actuator.damping, damping_default * torch.ones(num_envs, num_joints, device=device))
torch.testing.assert_close(
actuator.armature, armature_default * torch.ones(num_envs, num_joints, device=device)
)
torch.testing.assert_close(
actuator.friction, friction_default * torch.ones(num_envs, num_joints, device=device)
)


@pytest.mark.parametrize("num_envs", [1, 2])
@pytest.mark.parametrize("num_joints", [1, 2])
@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
@pytest.mark.parametrize("effort_lim", [None, 300])
@pytest.mark.parametrize("effort_lim_sim", [None, 400])
def test_ideal_pd_actuator_init_effort_limits(num_envs, num_joints, device, effort_lim, effort_lim_sim):
"""Test initialization of ideal pd actuator with effort limits."""
# used as a standin for the usd default value read in by articulation.
# This value should not be propagated for ideal pd actuators
effort_lim_default = 5000

joint_names = [f"joint_{d}" for d in range(num_joints)]
joint_ids = [d for d in range(num_joints)]

actuator_cfg = IdealPDActuatorCfg(
joint_names_expr=joint_names,
stiffness=200,
damping=10,
effort_limit=effort_lim,
effort_limit_sim=effort_lim_sim,
)

actuator = actuator_cfg.class_type(
actuator_cfg,
joint_names=joint_names,
joint_ids=joint_ids,
num_envs=num_envs,
device=device,
stiffness=actuator_cfg.stiffness,
damping=actuator_cfg.damping,
effort_limit=effort_lim_default,
)

if effort_lim is not None and effort_lim_sim is None:
effort_lim_expected = effort_lim
effort_lim_sim_expected = actuator._DEFAULT_MAX_EFFORT_SIM

elif effort_lim is None and effort_lim_sim is not None:
effort_lim_expected = effort_lim_sim
effort_lim_sim_expected = effort_lim_sim

elif effort_lim is None and effort_lim_sim is None:
effort_lim_expected = actuator._DEFAULT_MAX_EFFORT_SIM
effort_lim_sim_expected = actuator._DEFAULT_MAX_EFFORT_SIM

elif effort_lim is not None and effort_lim_sim is not None:
effort_lim_expected = effort_lim
effort_lim_sim_expected = effort_lim_sim

torch.testing.assert_close(
actuator.effort_limit, effort_lim_expected * torch.ones(num_envs, num_joints, device=device)
)
torch.testing.assert_close(
actuator.effort_limit_sim, effort_lim_sim_expected * torch.ones(num_envs, num_joints, device=device)
)


@pytest.mark.parametrize("num_envs", [1, 2])
@pytest.mark.parametrize("num_joints", [1, 2])
@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
@pytest.mark.parametrize("velocity_lim", [None, 300])
@pytest.mark.parametrize("velocity_lim_sim", [None, 400])
def test_ideal_pd_actuator_init_velocity_limits(num_envs, num_joints, device, velocity_lim, velocity_lim_sim):
"""Test initialization of ideal pd actuator with velocity limits.
Note Ideal PD actuator does not use velocity limits in computation, they are passed to physics via articulations.
"""
velocity_limit_default = 1000
joint_names = [f"joint_{d}" for d in range(num_joints)]
joint_ids = [d for d in range(num_joints)]

actuator_cfg = IdealPDActuatorCfg(
joint_names_expr=joint_names,
stiffness=200,
damping=10,
velocity_limit=velocity_lim,
velocity_limit_sim=velocity_lim_sim,
)

actuator = actuator_cfg.class_type(
actuator_cfg,
joint_names=joint_names,
joint_ids=joint_ids,
num_envs=num_envs,
device=device,
stiffness=actuator_cfg.stiffness,
damping=actuator_cfg.damping,
velocity_limit=velocity_limit_default,
)
if velocity_lim is not None and velocity_lim_sim is None:
vel_lim_expected = velocity_lim
vel_lim_sim_expected = velocity_limit_default
elif velocity_lim is None and velocity_lim_sim is not None:
vel_lim_expected = velocity_lim_sim
vel_lim_sim_expected = velocity_lim_sim
elif velocity_lim is None and velocity_lim_sim is None:
vel_lim_expected = velocity_limit_default
vel_lim_sim_expected = velocity_limit_default
elif velocity_lim is not None and velocity_lim_sim is not None:
vel_lim_expected = velocity_lim
vel_lim_sim_expected = velocity_lim_sim

torch.testing.assert_close(
actuator.velocity_limit, vel_lim_expected * torch.ones(num_envs, num_joints, device=device)
)
torch.testing.assert_close(
actuator.velocity_limit_sim, vel_lim_sim_expected * torch.ones(num_envs, num_joints, device=device)
)


@pytest.mark.parametrize("num_envs", [1, 2])
@pytest.mark.parametrize("num_joints", [1, 2])
@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
@pytest.mark.parametrize("effort_lim", [None, 300])
def test_ideal_pd_compute(num_envs, num_joints, device, effort_lim):
"""Test the computation of the ideal pd actuator."""

joint_names = [f"joint_{d}" for d in range(num_joints)]
joint_ids = [d for d in range(num_joints)]
stiffness = 200
damping = 10
actuator_cfg = IdealPDActuatorCfg(
joint_names_expr=joint_names,
stiffness=stiffness,
damping=damping,
effort_limit=effort_lim,
)

actuator = actuator_cfg.class_type(
actuator_cfg,
joint_names=joint_names,
joint_ids=joint_ids,
num_envs=num_envs,
device=device,
stiffness=actuator_cfg.stiffness,
damping=actuator_cfg.damping,
)
desired_pos = 10.0
desired_vel = 0.1
measured_joint_pos = 1.0
measured_joint_vel = -0.1

desired_control_action = ArticulationActions()
desired_control_action.joint_positions = desired_pos * torch.ones(num_envs, num_joints, device=device)
desired_control_action.joint_velocities = desired_vel * torch.ones(num_envs, num_joints, device=device)
desired_control_action.joint_efforts = torch.zeros(num_envs, num_joints, device=device)

expected_comp_joint_effort = stiffness * (desired_pos - measured_joint_pos) + damping * (
desired_vel - measured_joint_vel
)

computed_control_action = actuator.compute(
desired_control_action,
measured_joint_pos * torch.ones(num_envs, num_joints, device=device),
measured_joint_vel * torch.ones(num_envs, num_joints, device=device),
)

torch.testing.assert_close(
expected_comp_joint_effort * torch.ones(num_envs, num_joints, device=device), actuator.computed_effort
)

if effort_lim is None:
torch.testing.assert_close(
expected_comp_joint_effort * torch.ones(num_envs, num_joints, device=device), actuator.applied_effort
)
else:
torch.testing.assert_close(
effort_lim * torch.ones(num_envs, num_joints, device=device), actuator.applied_effort
)
torch.testing.assert_close(
actuator.applied_effort,
computed_control_action.joint_efforts,
)


if __name__ == "__main__":
pytest.main([__file__, "-v", "--maxfail=1"])
243 changes: 243 additions & 0 deletions source/isaaclab/test/actuators/test_implicit_actuator.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,243 @@
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause

from isaaclab.app import AppLauncher

HEADLESS = True

# if not AppLauncher.instance():
simulation_app = AppLauncher(headless=HEADLESS).app

"""Rest of imports follows"""

import torch

import pytest

from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.sim import build_simulation_context


@pytest.fixture
def sim(request):
"""Create simulation context with the specified device."""
device = request.getfixturevalue("device")
with build_simulation_context(device=device) as sim:
sim._app_control_on_stop_handle = None
yield sim


@pytest.mark.parametrize("num_envs", [1, 2])
@pytest.mark.parametrize("num_joints", [1, 2])
@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
@pytest.mark.parametrize("usd_default", [False, True])
def test_implicit_actuator_init_minimum(sim, num_envs, num_joints, device, usd_default):
"""Test initialization of implicit actuator with minimum configuration."""

joint_names = [f"joint_{d}" for d in range(num_joints)]
joint_ids = [d for d in range(num_joints)]
stiffness = None if usd_default else 200
damping = None if usd_default else 10
friction = None if usd_default else 0.1
armature = None if usd_default else 0.2

actuator_cfg = ImplicitActuatorCfg(
joint_names_expr=joint_names,
stiffness=stiffness,
damping=damping,
armature=armature,
friction=friction,
)
# assume Articulation class:
# - finds joints (names and ids) associate with the provided joint_names_expr

# faux usd defaults
stiffness_default = 300
damping_default = 20
friction_default = 0.0
armature_default = 0.0

actuator = actuator_cfg.class_type(
actuator_cfg,
joint_names=joint_names,
joint_ids=joint_ids,
num_envs=num_envs,
device=device,
stiffness=stiffness_default,
damping=damping_default,
friction=friction_default,
armature=armature_default,
)

# check initialized actuator
assert actuator.is_implicit_model is True
# check device and shape
torch.testing.assert_close(actuator.computed_effort, torch.zeros(num_envs, num_joints, device=device))
torch.testing.assert_close(actuator.applied_effort, torch.zeros(num_envs, num_joints, device=device))

torch.testing.assert_close(actuator.effort_limit, torch.inf * torch.ones(num_envs, num_joints, device=device))
torch.testing.assert_close(actuator.effort_limit_sim, torch.inf * torch.ones(num_envs, num_joints, device=device))
torch.testing.assert_close(actuator.velocity_limit, torch.inf * torch.ones(num_envs, num_joints, device=device))
torch.testing.assert_close(actuator.velocity_limit_sim, torch.inf * torch.ones(num_envs, num_joints, device=device))

if not usd_default:
torch.testing.assert_close(actuator.stiffness, stiffness * torch.ones(num_envs, num_joints, device=device))
torch.testing.assert_close(actuator.damping, damping * torch.ones(num_envs, num_joints, device=device))
torch.testing.assert_close(actuator.armature, armature * torch.ones(num_envs, num_joints, device=device))
torch.testing.assert_close(actuator.friction, friction * torch.ones(num_envs, num_joints, device=device))
else:
torch.testing.assert_close(
actuator.stiffness, stiffness_default * torch.ones(num_envs, num_joints, device=device)
)
torch.testing.assert_close(actuator.damping, damping_default * torch.ones(num_envs, num_joints, device=device))
torch.testing.assert_close(
actuator.armature, armature_default * torch.ones(num_envs, num_joints, device=device)
)
torch.testing.assert_close(
actuator.friction, friction_default * torch.ones(num_envs, num_joints, device=device)
)


@pytest.mark.parametrize("num_envs", [1, 2])
@pytest.mark.parametrize("num_joints", [1, 2])
@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
@pytest.mark.parametrize("effort_lim", [None, 300, 200])
@pytest.mark.parametrize("effort_lim_sim", [None, 400, 200])
def test_implicit_actuator_init_effort_limits(sim, num_envs, num_joints, device, effort_lim, effort_lim_sim):
"""Test initialization of implicit actuator with effort limits."""
effort_limit_default = 5000

joint_names = [f"joint_{d}" for d in range(num_joints)]
joint_ids = [d for d in range(num_joints)]

actuator_cfg = ImplicitActuatorCfg(
joint_names_expr=joint_names,
stiffness=200,
damping=10,
effort_limit=effort_lim,
effort_limit_sim=effort_lim_sim,
)

if effort_lim is not None and effort_lim_sim is not None and effort_lim != effort_lim_sim:
with pytest.raises(ValueError):
actuator = actuator_cfg.class_type(
actuator_cfg,
joint_names=joint_names,
joint_ids=joint_ids,
num_envs=num_envs,
device=device,
stiffness=actuator_cfg.stiffness,
damping=actuator_cfg.damping,
effort_limit=effort_limit_default,
)
else:
actuator = actuator_cfg.class_type(
actuator_cfg,
joint_names=joint_names,
joint_ids=joint_ids,
num_envs=num_envs,
device=device,
stiffness=actuator_cfg.stiffness,
damping=actuator_cfg.damping,
effort_limit=effort_limit_default,
)
if effort_lim is not None and effort_lim_sim is None:
assert actuator.cfg.effort_limit_sim == actuator.cfg.effort_limit
effort_lim_expected = effort_lim
effort_lim_sim_expected = effort_lim

elif effort_lim is None and effort_lim_sim is not None:
assert actuator.cfg.effort_limit_sim == actuator.cfg.effort_limit
effort_lim_expected = effort_lim_sim
effort_lim_sim_expected = effort_lim_sim

elif effort_lim is None and effort_lim_sim is None:
assert actuator.cfg.effort_limit_sim is None
assert actuator.cfg.effort_limit is None
effort_lim_expected = effort_limit_default
effort_lim_sim_expected = effort_limit_default

elif effort_lim is not None and effort_lim_sim is not None:
assert actuator.cfg.effort_limit_sim == actuator.cfg.effort_limit
effort_lim_expected = effort_lim
effort_lim_sim_expected = effort_lim_sim

torch.testing.assert_close(
actuator.effort_limit, effort_lim_expected * torch.ones(num_envs, num_joints, device=device)
)
torch.testing.assert_close(
actuator.effort_limit_sim, effort_lim_sim_expected * torch.ones(num_envs, num_joints, device=device)
)


@pytest.mark.parametrize("num_envs", [1, 2])
@pytest.mark.parametrize("num_joints", [1, 2])
@pytest.mark.parametrize("device", ["cuda:0", "cpu"])
@pytest.mark.parametrize("velocity_lim", [None, 300, 200])
@pytest.mark.parametrize("velocity_lim_sim", [None, 400, 200])
def test_implicit_actuator_init_velocity_limits(sim, num_envs, num_joints, device, velocity_lim, velocity_lim_sim):
"""Test initialization of implicit actuator with velocity limits.
Note implicit actuators do no use velocity limits in computation, they are passed to physics via articulations.
"""
velocity_limit_default = 1000
joint_names = [f"joint_{d}" for d in range(num_joints)]
joint_ids = [d for d in range(num_joints)]

actuator_cfg = ImplicitActuatorCfg(
joint_names_expr=joint_names,
stiffness=200,
damping=10,
velocity_limit=velocity_lim,
velocity_limit_sim=velocity_lim_sim,
)

if velocity_lim is not None and velocity_lim_sim is not None and velocity_lim != velocity_lim_sim:
with pytest.raises(ValueError):
actuator = actuator_cfg.class_type(
actuator_cfg,
joint_names=joint_names,
joint_ids=joint_ids,
num_envs=num_envs,
device=device,
stiffness=actuator_cfg.stiffness,
damping=actuator_cfg.damping,
velocity_limit=velocity_limit_default,
)
else:
actuator = actuator_cfg.class_type(
actuator_cfg,
joint_names=joint_names,
joint_ids=joint_ids,
num_envs=num_envs,
device=device,
stiffness=actuator_cfg.stiffness,
damping=actuator_cfg.damping,
velocity_limit=velocity_limit_default,
)
if velocity_lim is not None and velocity_lim_sim is None:
assert actuator.cfg.velocity_limit is None
vel_lim_expected = velocity_limit_default
elif velocity_lim is None and velocity_lim_sim is not None:
assert actuator.cfg.velocity_limit == actuator.cfg.velocity_limit_sim
vel_lim_expected = velocity_lim_sim
elif velocity_lim is None and velocity_lim_sim is None:
assert actuator.cfg.velocity_limit is None
assert actuator.cfg.velocity_limit_sim is None
vel_lim_expected = velocity_limit_default
else:
assert actuator.cfg.velocity_limit == actuator.cfg.velocity_limit_sim
vel_lim_expected = velocity_lim_sim

torch.testing.assert_close(
actuator.velocity_limit, vel_lim_expected * torch.ones(num_envs, num_joints, device=device)
)
torch.testing.assert_close(
actuator.velocity_limit_sim, vel_lim_expected * torch.ones(num_envs, num_joints, device=device)
)


if __name__ == "__main__":
pytest.main([__file__, "-v", "--maxfail=1"])