From 1fa5a8aaa9975a1b4ba0ca11286c889d41bf295d Mon Sep 17 00:00:00 2001 From: James Tigue Date: Tue, 8 Apr 2025 15:18:22 -0400 Subject: [PATCH 1/7] add implicit actuator init and effort limit test --- source/isaaclab/setup.py | 3 + .../test/actuators/test_actuators_pd.py | 153 ++++++++++++++++++ 2 files changed, 156 insertions(+) create mode 100644 source/isaaclab/test/actuators/test_actuators_pd.py diff --git a/source/isaaclab/setup.py b/source/isaaclab/setup.py index 86e9e4e88cb..e217ff0a0e0 100644 --- a/source/isaaclab/setup.py +++ b/source/isaaclab/setup.py @@ -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"] diff --git a/source/isaaclab/test/actuators/test_actuators_pd.py b/source/isaaclab/test/actuators/test_actuators_pd.py new file mode 100644 index 00000000000..c8961eaf749 --- /dev/null +++ b/source/isaaclab/test/actuators/test_actuators_pd.py @@ -0,0 +1,153 @@ +# Copyright (c) 2022-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") + if "gravity_enabled" in request.fixturenames: + gravity_enabled = request.getfixturevalue("gravity_enabled") + else: + gravity_enabled = True # default to gravity enabled + if "add_ground_plane" in request.fixturenames: + add_ground_plane = request.getfixturevalue("add_ground_plane") + else: + add_ground_plane = False # default to no ground plane + with build_simulation_context( + device=device, auto_add_lighting=True, gravity_enabled=gravity_enabled, add_ground_plane=add_ground_plane + ) 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"]) +def test_implicit_actuator_init_minimum(sim, num_envs, num_joints, device): + """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 = 200 + damping = 10 + actuator_cfg = ImplicitActuatorCfg( + joint_names_expr=joint_names, + stiffness=stiffness, + damping=damping, + ) + # 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 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)) + + 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, torch.zeros(num_envs, num_joints, device=device)) + torch.testing.assert_close(actuator.friction, torch.zeros(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_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: + 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 + torch.testing.assert_close( + actuator.effort_limit, effort_lim * torch.ones(num_envs, num_joints, device=device) + ) + torch.testing.assert_close( + actuator.effort_limit_sim, effort_lim * torch.ones(num_envs, num_joints, device=device) + ) + elif effort_lim is None and effort_lim_sim is not None: + assert actuator.cfg.effort_limit_sim == actuator.cfg.effort_limit + torch.testing.assert_close( + actuator.effort_limit, effort_lim_sim * torch.ones(num_envs, num_joints, device=device) + ) + torch.testing.assert_close( + actuator.effort_limit_sim, effort_lim_sim * torch.ones(num_envs, num_joints, device=device) + ) + else: + assert actuator.cfg.effort_limit_sim is None + assert actuator.cfg.effort_limit is None + torch.testing.assert_close( + actuator.effort_limit, effort_limit_default * torch.ones(num_envs, num_joints, device=device) + ) + + +if __name__ == "__main__": + pytest.main([__file__, "-v", "--maxfail=1"]) From 3b9d9f7496d93f9ce98a68cd1972f30b165ce283 Mon Sep 17 00:00:00 2001 From: James Tigue Date: Tue, 8 Apr 2025 16:22:24 -0400 Subject: [PATCH 2/7] add edge cases and velocity limit cfg tests --- .../test/actuators/test_actuators_pd.py | 126 ++++++++++++++++-- 1 file changed, 115 insertions(+), 11 deletions(-) diff --git a/source/isaaclab/test/actuators/test_actuators_pd.py b/source/isaaclab/test/actuators/test_actuators_pd.py index c8961eaf749..2e3148bc36b 100644 --- a/source/isaaclab/test/actuators/test_actuators_pd.py +++ b/source/isaaclab/test/actuators/test_actuators_pd.py @@ -42,27 +42,43 @@ def sim(request): @pytest.mark.parametrize("num_envs", [1, 2]) @pytest.mark.parametrize("num_joints", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -def test_implicit_actuator_init_minimum(sim, num_envs, num_joints, device): +@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 = 200 - damping = 10 + 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 @@ -76,17 +92,29 @@ def test_implicit_actuator_init_minimum(sim, num_envs, num_joints, 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)) - 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, torch.zeros(num_envs, num_joints, device=device)) - torch.testing.assert_close(actuator.friction, torch.zeros(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]) +@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 @@ -102,7 +130,7 @@ def test_implicit_actuator_init_effort_limits(sim, num_envs, num_joints, device, effort_limit_sim=effort_lim_sim, ) - if effort_lim is not None and effort_lim_sim is not None: + 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, @@ -141,12 +169,88 @@ def test_implicit_actuator_init_effort_limits(sim, num_envs, num_joints, device, torch.testing.assert_close( actuator.effort_limit_sim, effort_lim_sim * torch.ones(num_envs, num_joints, device=device) ) - else: + 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 torch.testing.assert_close( actuator.effort_limit, effort_limit_default * torch.ones(num_envs, num_joints, device=device) ) + torch.testing.assert_close( + actuator.effort_limit_sim, effort_limit_default * torch.ones(num_envs, num_joints, device=device) + ) + + elif effort_lim is not None and effort_lim_sim is not None: + assert actuator.cfg.effort_limit_sim == actuator.cfg.effort_limit + torch.testing.assert_close( + actuator.effort_limit, effort_lim * 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 + expected_limit = 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 + expected_limit = 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 + expected_limit = velocity_limit_default + else: + assert actuator.cfg.velocity_limit == actuator.cfg.velocity_limit_sim + expected_limit = velocity_lim_sim + + torch.testing.assert_close( + actuator.velocity_limit, expected_limit * torch.ones(num_envs, num_joints, device=device) + ) + torch.testing.assert_close( + actuator.velocity_limit_sim, expected_limit * torch.ones(num_envs, num_joints, device=device) + ) if __name__ == "__main__": From 97ea7f0c586c16bbbce6fcc9d859ddec57b7a301 Mon Sep 17 00:00:00 2001 From: James Tigue Date: Wed, 9 Apr 2025 08:58:31 -0400 Subject: [PATCH 3/7] rename to test_implicit_actuator --- .../actuators/{test_actuators_pd.py => test_implicit_actuator.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename source/isaaclab/test/actuators/{test_actuators_pd.py => test_implicit_actuator.py} (100%) diff --git a/source/isaaclab/test/actuators/test_actuators_pd.py b/source/isaaclab/test/actuators/test_implicit_actuator.py similarity index 100% rename from source/isaaclab/test/actuators/test_actuators_pd.py rename to source/isaaclab/test/actuators/test_implicit_actuator.py From c1dab1ede6b575022d363de689f337be0cb5970a Mon Sep 17 00:00:00 2001 From: James Tigue Date: Wed, 9 Apr 2025 10:26:02 -0400 Subject: [PATCH 4/7] cleanup and add ideal_pd tests --- .../test/actuators/test_ideal_pd_actuator.py | 212 ++++++++++++++++++ .../test/actuators/test_implicit_actuator.py | 64 +++--- 2 files changed, 237 insertions(+), 39 deletions(-) create mode 100644 source/isaaclab/test/actuators/test_ideal_pd_actuator.py diff --git a/source/isaaclab/test/actuators/test_ideal_pd_actuator.py b/source/isaaclab/test/actuators/test_ideal_pd_actuator.py new file mode 100644 index 00000000000..1339ec17489 --- /dev/null +++ b/source/isaaclab/test/actuators/test_ideal_pd_actuator.py @@ -0,0 +1,212 @@ +# 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.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_ideal_pd_actuator_init_minimum(sim, 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(sim, num_envs, num_joints, device, effort_lim, effort_lim_sim): + """Test initialization of ideal pd actuator with effort limits.""" + 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(sim, 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) + ) diff --git a/source/isaaclab/test/actuators/test_implicit_actuator.py b/source/isaaclab/test/actuators/test_implicit_actuator.py index 2e3148bc36b..76f754e22f6 100644 --- a/source/isaaclab/test/actuators/test_implicit_actuator.py +++ b/source/isaaclab/test/actuators/test_implicit_actuator.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# Copyright (c) 2025, The Isaac Lab Project Developers. # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause @@ -24,17 +24,7 @@ def sim(request): """Create simulation context with the specified device.""" device = request.getfixturevalue("device") - if "gravity_enabled" in request.fixturenames: - gravity_enabled = request.getfixturevalue("gravity_enabled") - else: - gravity_enabled = True # default to gravity enabled - if "add_ground_plane" in request.fixturenames: - add_ground_plane = request.getfixturevalue("add_ground_plane") - else: - add_ground_plane = False # default to no ground plane - with build_simulation_context( - device=device, auto_add_lighting=True, gravity_enabled=gravity_enabled, add_ground_plane=add_ground_plane - ) as sim: + with build_simulation_context(device=device) as sim: sim._app_control_on_stop_handle = None yield sim @@ -155,35 +145,31 @@ def test_implicit_actuator_init_effort_limits(sim, num_envs, num_joints, device, ) if effort_lim is not None and effort_lim_sim is None: assert actuator.cfg.effort_limit_sim == actuator.cfg.effort_limit - torch.testing.assert_close( - actuator.effort_limit, effort_lim * torch.ones(num_envs, num_joints, device=device) - ) - torch.testing.assert_close( - actuator.effort_limit_sim, effort_lim * torch.ones(num_envs, num_joints, device=device) - ) + 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 - torch.testing.assert_close( - actuator.effort_limit, effort_lim_sim * torch.ones(num_envs, num_joints, device=device) - ) - torch.testing.assert_close( - actuator.effort_limit_sim, effort_lim_sim * torch.ones(num_envs, num_joints, device=device) - ) + 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 - torch.testing.assert_close( - actuator.effort_limit, effort_limit_default * torch.ones(num_envs, num_joints, device=device) - ) - torch.testing.assert_close( - actuator.effort_limit_sim, effort_limit_default * torch.ones(num_envs, num_joints, device=device) - ) + 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 - torch.testing.assert_close( - actuator.effort_limit, effort_lim * torch.ones(num_envs, num_joints, device=device) - ) + 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]) @@ -233,23 +219,23 @@ def test_implicit_actuator_init_velocity_limits(sim, num_envs, num_joints, devic ) if velocity_lim is not None and velocity_lim_sim is None: assert actuator.cfg.velocity_limit is None - expected_limit = velocity_limit_default + 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 - expected_limit = velocity_lim_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 - expected_limit = velocity_limit_default + vel_lim_expected = velocity_limit_default else: assert actuator.cfg.velocity_limit == actuator.cfg.velocity_limit_sim - expected_limit = velocity_lim_sim + vel_lim_expected = velocity_lim_sim torch.testing.assert_close( - actuator.velocity_limit, expected_limit * torch.ones(num_envs, num_joints, device=device) + actuator.velocity_limit, vel_lim_expected * torch.ones(num_envs, num_joints, device=device) ) torch.testing.assert_close( - actuator.velocity_limit_sim, expected_limit * torch.ones(num_envs, num_joints, device=device) + actuator.velocity_limit_sim, vel_lim_expected * torch.ones(num_envs, num_joints, device=device) ) From cddbeb57ad41ad6ead0a274c6a9a5cf610568709 Mon Sep 17 00:00:00 2001 From: James Tigue Date: Wed, 9 Apr 2025 10:57:09 -0400 Subject: [PATCH 5/7] add ideal_pd compute test --- .../test/actuators/test_ideal_pd_actuator.py | 72 +++++++++++++++++++ 1 file changed, 72 insertions(+) diff --git a/source/isaaclab/test/actuators/test_ideal_pd_actuator.py b/source/isaaclab/test/actuators/test_ideal_pd_actuator.py index 1339ec17489..aed9364e7dd 100644 --- a/source/isaaclab/test/actuators/test_ideal_pd_actuator.py +++ b/source/isaaclab/test/actuators/test_ideal_pd_actuator.py @@ -18,6 +18,7 @@ from isaaclab.actuators import IdealPDActuatorCfg from isaaclab.sim import build_simulation_context +from isaaclab.utils.types import ArticulationActions @pytest.fixture @@ -111,6 +112,8 @@ def test_ideal_pd_actuator_init_minimum(sim, num_envs, num_joints, device, usd_d @pytest.mark.parametrize("effort_lim_sim", [None, 400]) def test_ideal_pd_actuator_init_effort_limits(sim, 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)] @@ -210,3 +213,72 @@ def test_ideal_pd_actuator_init_velocity_limits(sim, num_envs, num_joints, devic 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_actuator_compute(sim, 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"]) From 283ef5ccb52ed31f505d5dbd0758215d80a71ee8 Mon Sep 17 00:00:00 2001 From: James Tigue Date: Thu, 10 Apr 2025 09:30:56 -0400 Subject: [PATCH 6/7] dc motor clip test for positive power --- .../test/actuators/test_ideal_pd_actuator.py | 135 +++++++++++++++++- 1 file changed, 133 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/test/actuators/test_ideal_pd_actuator.py b/source/isaaclab/test/actuators/test_ideal_pd_actuator.py index aed9364e7dd..6143c31e282 100644 --- a/source/isaaclab/test/actuators/test_ideal_pd_actuator.py +++ b/source/isaaclab/test/actuators/test_ideal_pd_actuator.py @@ -12,11 +12,12 @@ """Rest of imports follows""" +import math import torch import pytest -from isaaclab.actuators import IdealPDActuatorCfg +from isaaclab.actuators import DCMotorCfg, IdealPDActuatorCfg from isaaclab.sim import build_simulation_context from isaaclab.utils.types import ArticulationActions @@ -219,7 +220,7 @@ def test_ideal_pd_actuator_init_velocity_limits(sim, num_envs, num_joints, devic @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_actuator_compute(sim, num_envs, num_joints, device, effort_lim): +def test_ideal_pd_compute(sim, 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)] @@ -280,5 +281,135 @@ def test_ideal_pd_actuator_compute(sim, num_envs, num_joints, device, effort_lim ) +@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, 60]) +@pytest.mark.parametrize("saturation_effort", [None, 100]) +@pytest.mark.parametrize("mirror_t_s", [1.0, -1.0]) +@pytest.mark.parametrize("test_point", range(4)) +def test_dc_motor_clip(sim, num_envs, num_joints, device, effort_lim, saturation_effort, mirror_t_s, 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) + + e - effort_limit + s - saturation_effort + v - velocity_limit + \ - torque-speed linear boundary between v and s + + each torque_speed_point will be tested in quadrant 3 and 4 + + ===========Speed============== + |\ 6 | | | | + | \ | | | | + | \ | | | | + | \ 4 | | | + | Q2 | \ | | Q1 | + | | \ | 3 | | + | | v | | + | | | \ | 2 | + | | | \ | | + | | | \ | + |\ | 5 | 0 |1\ | + |--s---e-----o-----e---s-----| Torque + | \ | | | \ | + | \ | | \ | + | | \ | | | + | Q3 | \ | | Q4 | + | | v | | + | | | \ | | + ============================== + """ + torque_speed_pairs = [ + (30.0, 10.0), + (70.0, 10.0), + (80.0, 40.0), + (30.0, 40.0), + (-20.0, 90.0), + (-30.0, 10.0), + (-80.0, 110.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 + 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, + ) + + i = test_point + ts = torque_speed_pairs[test_point] + torque = ts[0] * mirror_t_s + speed = ts[1] * mirror_t_s + 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) + + if saturation_effort is not None: + torque_speed_curve = saturation_effort * (mirror_t_s * 1 - speed / velocity_limit) + + if i == 0 or i == 5: + expected_clipped_effort = torque + elif i == 1: + expected_clipped_effort = math.copysign(effort_lim, torque) if effort_lim is not None else torque + elif i == 2: + if saturation_effort is not None: + expected_clipped_effort = torque_speed_curve + elif effort_lim is not None: + expected_clipped_effort = math.copysign(effort_lim, torque) + else: + expected_clipped_effort = torque + elif i == 3: + if saturation_effort is not None: + expected_clipped_effort = torque_speed_curve + else: + expected_clipped_effort = torque + elif i == 4: + if effort_lim is not None: + expected_clipped_effort = math.copysign(effort_lim, torque) + elif saturation_effort is not None: + expected_clipped_effort = torque_speed_curve + else: + expected_clipped_effort = torque + print("expected: ", expected_clipped_effort) + print("clipped:", clipped_effort) + elif i == 6: + if effort_lim is not None: + expected_clipped_effort = math.copysign(effort_lim, torque) + elif saturation_effort is not None: + expected_clipped_effort = torque_speed_curve + else: + expected_clipped_effort = torque + + torch.testing.assert_close( + expected_clipped_effort * torch.ones(num_envs, num_joints, device=device), clipped_effort + ) + + if __name__ == "__main__": pytest.main([__file__, "-v", "--maxfail=1"]) From ca7b1a00bb8986e6a33fa39b5707a3a34e097b6c Mon Sep 17 00:00:00 2001 From: James Tigue Date: Thu, 10 Apr 2025 09:38:16 -0400 Subject: [PATCH 7/7] change log --- source/isaaclab/config/extension.toml | 2 +- source/isaaclab/docs/CHANGELOG.rst | 10 ++++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index b47bd022ef4..2afe4982150 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -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" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 7ab5e86c40f..257ea7880a1 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,16 @@ Changelog --------- +0.36.5 (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` + + 0.36.4 (2025-03-24) ~~~~~~~~~~~~~~~~~~~