From e5588dbdda017813b9ee53e32b4bd645974edd85 Mon Sep 17 00:00:00 2001 From: Maurice Rahme Date: Tue, 12 Aug 2025 00:54:13 -0400 Subject: [PATCH 1/4] fix(terminations): index before reduce in joint_pos_out_of_limit MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Root cause: reduced over joints to [N] then indexed with [:, joint_ids] → IndexError. Change: compare to limits → [N,J], slice columns with joint_ids → [N,K], then .any(dim=1). Result: returns [N] without error for both [1,J,2] and [N,J,2] limit tensors; joint_ids=None still uses all joints. --- .../isaaclab/isaaclab/envs/mdp/terminations.py | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/terminations.py b/source/isaaclab/isaaclab/envs/mdp/terminations.py index 9f0f7858535..5b8d8426964 100644 --- a/source/isaaclab/isaaclab/envs/mdp/terminations.py +++ b/source/isaaclab/isaaclab/envs/mdp/terminations.py @@ -84,10 +84,19 @@ def joint_pos_out_of_limit(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = S if asset_cfg.joint_ids is None: asset_cfg.joint_ids = slice(None) - limits = asset.data.soft_joint_pos_limits[:, asset_cfg.joint_ids] - out_of_upper_limits = torch.any(asset.data.joint_pos[:, asset_cfg.joint_ids] > limits[..., 1], dim=1) - out_of_lower_limits = torch.any(asset.data.joint_pos[:, asset_cfg.joint_ids] < limits[..., 0], dim=1) - return torch.logical_or(out_of_upper_limits, out_of_lower_limits) + # compute any per-joint violations (avoid reducing before indexing) + out_of_upper_limits = asset.data.joint_pos > asset.data.soft_joint_pos_limits[..., 1] # [N, J] + out_of_lower_limits = asset.data.joint_pos < asset.data.soft_joint_pos_limits[..., 0] # [N, J] + + # truncate above output to just the joints we care about + out_of_upper_limits = out_of_upper_limits[:, asset_cfg.joint_ids] # [N, K] + out_of_lower_limits = out_of_lower_limits[:, asset_cfg.joint_ids] # [N, K] + + # reduce over selected joints + out_of_upper_limits = torch.any(out_of_upper_limits, dim=1) # [N] + out_of_lower_limits = torch.any(out_of_lower_limits, dim=1) # [N] + + return torch.logical_or(out_of_upper_limits, out_of_lower_limits) # [N] def joint_pos_out_of_manual_limit( From 89fe1750b0c5ae6678a5cac3c614a1c3935f1350 Mon Sep 17 00:00:00 2001 From: Maurice Rahme Date: Tue, 12 Aug 2025 01:00:07 -0400 Subject: [PATCH 2/4] update contributors --- CONTRIBUTORS.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index f916a70750b..6a1f37d9c57 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -89,6 +89,7 @@ Guidelines for modifications: * Lukas Fröhlich * Manuel Schweiger * Masoud Moghani +* Maurice Rahme * Michael Gussert * Michael Noseworthy * Miguel Alonso Jr From a3228bf78a03b853f4b8240485bbebe3e064afc1 Mon Sep 17 00:00:00 2001 From: Maurice Rahme Date: Tue, 12 Aug 2025 01:08:38 -0400 Subject: [PATCH 3/4] update unit test to use joint_pos_out_of_limit --- .../isaaclab/test/assets/test_articulation.py | 53 ++++++++++--------- 1 file changed, 28 insertions(+), 25 deletions(-) diff --git a/source/isaaclab/test/assets/test_articulation.py b/source/isaaclab/test/assets/test_articulation.py index 11bcbf01243..3a9d6cbdb6c 100644 --- a/source/isaaclab/test/assets/test_articulation.py +++ b/source/isaaclab/test/assets/test_articulation.py @@ -31,6 +31,8 @@ from isaaclab.assets import Articulation, ArticulationCfg from isaaclab.sim import build_simulation_context from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.managers import SceneEntityCfg +from isaaclab.envs.mdp.terminations import joint_pos_out_of_limit ## # Pre-defined configs @@ -657,25 +659,21 @@ def test_out_of_range_default_joint_vel(sim, device): @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) def test_joint_pos_limits(sim, num_articulations, device, add_ground_plane): - """Test write_joint_limits_to_sim API and when default pos falls outside of the new limits. - - This test verifies that: - 1. Joint limits can be set correctly - 2. Default positions are preserved when setting new limits - 3. Joint limits can be set with indexing - 4. Invalid joint positions are properly handled - - Args: - sim: The simulation fixture - num_articulations: Number of articulations to test - """ + """Test write_joint_limits_to_sim API and validate limits via joint_pos_out_of_limit().""" # Create articulation articulation_cfg = generate_articulation_cfg(articulation_type="panda") articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) + # Minimal fake env that exposes scene["robot"] -> articulation + class _Env: + def __init__(self, art): + self.scene = {"robot": art} + + env = _Env(articulation) + robot_all = SceneEntityCfg(name="robot") # all joints + # Play sim sim.reset() - # Check if articulation is initialized assert articulation.is_initialized # Get current default joint pos @@ -691,6 +689,10 @@ def test_joint_pos_limits(sim, num_articulations, device, add_ground_plane): torch.testing.assert_close(articulation._data.joint_pos_limits, limits) torch.testing.assert_close(articulation._data.default_joint_pos, default_joint_pos) + # Validate via function: no joint should be out of limits + out = joint_pos_out_of_limit(env, robot_all) # [N] + assert torch.all(~out) + # Set new joint limits with indexing env_ids = torch.arange(1, device=device) joint_ids = torch.arange(2, device=device) @@ -703,29 +705,30 @@ def test_joint_pos_limits(sim, num_articulations, device, add_ground_plane): torch.testing.assert_close(articulation._data.joint_pos_limits[env_ids][:, joint_ids], limits) torch.testing.assert_close(articulation._data.default_joint_pos, default_joint_pos) - # Set new joint limits that invalidate default joint pos + # Validate via function on selected joints + robot_subset = SceneEntityCfg(name="robot", joint_ids=joint_ids.tolist()) + out = joint_pos_out_of_limit(env, robot_subset) # [N] + assert torch.all(~out) + + # Set new joint limits that (narrowly) constrain default joint pos limits = torch.zeros(num_articulations, articulation.num_joints, 2, device=device) limits[..., 0] = torch.rand(num_articulations, articulation.num_joints, device=device) * -0.1 limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) * 0.1 articulation.write_joint_position_limit_to_sim(limits) - # Check if all values are within the bounds - within_bounds = (articulation._data.default_joint_pos >= limits[..., 0]) & ( - articulation._data.default_joint_pos <= limits[..., 1] - ) - assert torch.all(within_bounds) + # Validate via function: defaults should still be within new limits + out = joint_pos_out_of_limit(env, robot_all) # [N] + assert torch.all(~out) - # Set new joint limits that invalidate default joint pos with indexing + # Set new joint limits that constrain default joint pos with indexing limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device) limits[..., 0] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * -0.1 limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * 0.1 articulation.write_joint_position_limit_to_sim(limits, env_ids=env_ids, joint_ids=joint_ids) - # Check if all values are within the bounds - within_bounds = (articulation._data.default_joint_pos[env_ids][:, joint_ids] >= limits[..., 0]) & ( - articulation._data.default_joint_pos[env_ids][:, joint_ids] <= limits[..., 1] - ) - assert torch.all(within_bounds) + # Validate via function on selected joints + out = joint_pos_out_of_limit(env, robot_subset) # [N] + assert torch.all(~out) @pytest.mark.parametrize("num_articulations", [1, 2]) From f5b9ff4f3808d8499800d9cf63a64e4c79817cc8 Mon Sep 17 00:00:00 2001 From: Maurice Rahme Date: Tue, 12 Aug 2025 01:10:49 -0400 Subject: [PATCH 4/4] formatting --- .../isaaclab/test/assets/test_articulation.py | 23 ++++++++++++++----- 1 file changed, 17 insertions(+), 6 deletions(-) diff --git a/source/isaaclab/test/assets/test_articulation.py b/source/isaaclab/test/assets/test_articulation.py index 3a9d6cbdb6c..42263267948 100644 --- a/source/isaaclab/test/assets/test_articulation.py +++ b/source/isaaclab/test/assets/test_articulation.py @@ -29,10 +29,10 @@ import isaaclab.utils.string as string_utils from isaaclab.actuators import ActuatorBase, IdealPDActuatorCfg, ImplicitActuatorCfg from isaaclab.assets import Articulation, ArticulationCfg +from isaaclab.envs.mdp.terminations import joint_pos_out_of_limit +from isaaclab.managers import SceneEntityCfg from isaaclab.sim import build_simulation_context from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR -from isaaclab.managers import SceneEntityCfg -from isaaclab.envs.mdp.terminations import joint_pos_out_of_limit ## # Pre-defined configs @@ -659,7 +659,16 @@ def test_out_of_range_default_joint_vel(sim, device): @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) def test_joint_pos_limits(sim, num_articulations, device, add_ground_plane): - """Test write_joint_limits_to_sim API and validate limits via joint_pos_out_of_limit().""" + """Test write_joint_limits_to_sim API and when default pos falls outside of the new limits. + This test verifies that: + 1. Joint limits can be set correctly + 2. Default positions are preserved when setting new limits + 3. Joint limits can be set with indexing + 4. Invalid joint positions are properly handled + Args: + sim: The simulation fixture + num_articulations: Number of articulations to test + """ # Create articulation articulation_cfg = generate_articulation_cfg(articulation_type="panda") articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) @@ -674,6 +683,7 @@ def __init__(self, art): # Play sim sim.reset() + # Check if articulation is initialized assert articulation.is_initialized # Get current default joint pos @@ -689,6 +699,7 @@ def __init__(self, art): torch.testing.assert_close(articulation._data.joint_pos_limits, limits) torch.testing.assert_close(articulation._data.default_joint_pos, default_joint_pos) + # Set new joint limits that invalidate default joint pos # Validate via function: no joint should be out of limits out = joint_pos_out_of_limit(env, robot_all) # [N] assert torch.all(~out) @@ -705,7 +716,7 @@ def __init__(self, art): torch.testing.assert_close(articulation._data.joint_pos_limits[env_ids][:, joint_ids], limits) torch.testing.assert_close(articulation._data.default_joint_pos, default_joint_pos) - # Validate via function on selected joints + # Set new joint limits that invalidate default joint pos robot_subset = SceneEntityCfg(name="robot", joint_ids=joint_ids.tolist()) out = joint_pos_out_of_limit(env, robot_subset) # [N] assert torch.all(~out) @@ -716,11 +727,11 @@ def __init__(self, art): limits[..., 1] = torch.rand(num_articulations, articulation.num_joints, device=device) * 0.1 articulation.write_joint_position_limit_to_sim(limits) - # Validate via function: defaults should still be within new limits + # Check if all values are within the bounds out = joint_pos_out_of_limit(env, robot_all) # [N] assert torch.all(~out) - # Set new joint limits that constrain default joint pos with indexing + # Set new joint limits that invalidate default joint pos with indexing limits = torch.zeros(env_ids.shape[0], joint_ids.shape[0], 2, device=device) limits[..., 0] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * -0.1 limits[..., 1] = torch.rand(env_ids.shape[0], joint_ids.shape[0], device=device) * 0.1