Unverified Commit 24a78b68 authored by James Tigue's avatar James Tigue Committed by GitHub

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

# Description

This PR adds tests for actuator initialization, configuration, and
computation for Implicit, IdealPD, and DC motor actuators.

This PR also updates the DCMotor model to clip based on a four quadrant
DC motor model. This will fix improper clipping in the negative power
regions (i.e. positive torque and negative velocity or negative torque
and positive velocity).

NOTE: This PR is dependant on the pytest migration in:
[2034](https://github.com/isaac-sim/IsaacLab/pull/2034)
This PR includes changes made in
[2291](https://github.com/isaac-sim/IsaacLab/pull/2291) and would be an
alternate candidate.

Fixes [#2139](https://github.com/isaac-sim/IsaacLab/issues/2139)

<!-- 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


![image](https://github.com/user-attachments/assets/c94f877e-b3a9-441a-ad2e-ec6124cc64de)

## 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
- [x] 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 avatarKelly Guo <kellyg@nvidia.com>
Signed-off-by: 's avatarJames Tigue <166445701+jtigue-bdai@users.noreply.github.com>
Signed-off-by: 's avatarKelly Guo <kellyguo123@hotmail.com>
Co-authored-by: 's avatarKelly Guo <kellyg@nvidia.com>
Co-authored-by: 's avatarKelly Guo <kellyguo123@hotmail.com>
parent ce2ae923
......@@ -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:
......
[package]
# Note: Semantic Versioning is used: https://semver.org/
version = "0.41.0"
version = "0.41.1"
# Description
title = "Isaac Lab framework for Robot Learning"
......
Changelog
---------
0.41.1 (2025-07-22)
~~~~~~~~~~~~~~~~~~~
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.41.0 (2025-07-21)
~~~~~~~~~~~~~~~~~~~
......
......@@ -79,8 +79,6 @@ class ActuatorNetLSTM(DCMotor):
# 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
self._joint_vel[:] = joint_vel
# run network inference
with torch.inference_mode():
......
......@@ -201,8 +201,8 @@ class IdealPDActuator(ActuatorBase):
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 for velocities between corner velocities
(where torque-speed curve intersects with continuous torque) are defined as follows:
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 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,10 @@ 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 Continuous Torque (`effort_limit`).
The figure below demonstrates the clipping action for example (velocity, torque) pairs.
"""
cfg: DCMotorCfg
......@@ -245,10 +254,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:
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
else:
self._saturation_effort = torch.inf
# 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
......@@ -274,16 +284,18 @@ class DCMotor(IdealPDActuator):
"""
def _clip_effort(self, effort: torch.Tensor) -> torch.Tensor:
# save current joint vel
self._joint_vel[:] = torch.clip(self._joint_vel, min=-self._vel_at_effort_lim, max=self._vel_at_effort_lim)
# 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)
return clamped
class DelayedPDActuator(IdealPDActuator):
......
# Copyright (c) 2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# 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", "cpu"])
@pytest.mark.parametrize("test_point", range(20))
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 and outside corner velocity(quadrant 4)
5 - fully inside torque speed curve and effort limit (quadrant 4)
6 - fully outside torque speed curve and -effort limit (quadrant 4)
7 - fully inside torque speed curve, outside -effort limit, and inside corner velocity (quadrant 4)
8 - fully inside torque speed curves, outside -effort limit, and outside corner velocity (quadrant 4)
9 - less than effort limit but outside torque speed curve and inside corner velocity (quadrant 4)
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
\ | \ 9 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
(-10.0, 70.0), # 9
(-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
(10.0, -70.0), # -9
]
expected_clipped_effort = [
30.0, # 0
60.0, # 1
20.0, # 2
20.0, # 3
-60.0, # 4
-30.0, # 5
-60.0, # 6
-60.0, # 7
-60.0, # 8
-40.0, # 9
-30.0, # -0
-60.0, # -1
-20, # -2
-20, # -3
60.0, # -4
30.0, # -5
60.0, # -6
60.0, # -7
60.0, # -8
40.0, # -9
]
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 = 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,
)
This diff is collapsed.
# Copyright (c) 2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
# 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"])
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