Fixes external force buffers to set to zero when no forces/torques are applied (#185)

<!--
Thank you for your interest in sending a pull request. Please make sure
to check the contribution guidelines.

Link: https://isaac-sim.github.io/IsaacLab/source/refs/contributing.html
-->

Fixes an issue where when a zero value wrench is input to the
`set_external_force_and_torque` function, the force and torque buffer
arrays are not properly updated. Also added unit tests.

Fixes https://github.com/isaac-sim/IsaacLab/issues/262

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

<!-- As you go through the list, delete the ones that are not
applicable. -->

- Bug fix (non-breaking change which fixes an issue)

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [ ] I have made corresponding changes to the documentation
- [ ] 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: Kelly Guo <kellyg@nvidia.com>
Co-authored-by: Kelly Guo <kellyg@nvidia.com>

CONTRIBUTORS.md

@@ -26,6 +26,7 @@ Guidelines for modifications:
 * James Smith
 * James Tigue
 * Kelly (Yunrong) Guo
+* Matthew Trepte
 * Mayank Mittal
 * Nikita Rudin
 * Pascal Roth

source/extensions/omni.isaac.lab/docs/CHANGELOG.rst

@@ -163,7 +163,7 @@ Added
 * Added full buffer property to :class:`omni.isaac.lab.utils.buffers.circular_buffer.CircularBuffer`
 
 
-0.28.3 (2024-12-15)
+0.28.4 (2024-12-15)
 ~~~~~~~~~~~~~~~~~~~
 
 Added
@@ -172,7 +172,7 @@ Added
 * Added action clip to all :class:`omni.isaac.lab.envs.mdp.actions`.
 
 
-0.28.2 (2024-12-14)
+0.28.3 (2024-12-14)
 ~~~~~~~~~~~~~~~~~~~
 
 Changed
@@ -181,7 +181,7 @@ Changed
 * Added check for error below threshold in state machines to ensure the state has been reached.
 
 
-0.28.1 (2024-12-13)
+0.28.2 (2024-12-13)
 ~~~~~~~~~~~~~~~~~~~
 
 Fixed
@@ -190,6 +190,15 @@ Fixed
 * Fixed the shape of ``quat_w`` in the ``apply_actions`` method of :attr:`~omni.isaac.lab.env.mdp.NonHolonomicAction` (previously (N,B,4), now (N,4) since the number of root bodies B is required to be 1). Previously ``apply_actions`` errored because ``euler_xyz_from_quat`` requires inputs of shape (N,4).
 
 
+0.28.1 (2024-12-13)
+~~~~~~~~~~~~~~~~~~~
+
+Fixed
+^^^^^
+
+* Fixed the internal buffers for ``set_external_force_and_torque`` where the buffer values would be stale if zero values are sent to the APIs.
+
+
 0.28.0 (2024-12-12)
 ~~~~~~~~~~~~~~~~~~~
 

source/extensions/omni.isaac.lab/omni/isaac/lab/assets/articulation/articulation.py

@@ -767,6 +767,9 @@ class Articulation(AssetBase):
         """
         if forces.any() or torques.any():
             self.has_external_wrench = True
+        else:
+            self.has_external_wrench = False
+
         # resolve all indices
         # -- env_ids
         if env_ids is None:
@@ -790,8 +793,6 @@ class Articulation(AssetBase):
         # note: these are applied in the write_to_sim function
         self._external_force_b.flatten(0, 1)[indices] = forces.flatten(0, 1)
         self._external_torque_b.flatten(0, 1)[indices] = torques.flatten(0, 1)
-        else:
-            self.has_external_wrench = False
 
     def set_joint_position_target(
         self, target: torch.Tensor, joint_ids: Sequence[int] | slice | None = None, env_ids: Sequence[int] | None = None

source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object/rigid_object.py

@@ -376,6 +376,11 @@ class RigidObject(AssetBase):
         """
         if forces.any() or torques.any():
             self.has_external_wrench = True
+        else:
+            self.has_external_wrench = False
+            # to be safe, explicitly set value to zero
+            forces = torques = 0.0
+
         # resolve all indices
         # -- env_ids
         if env_ids is None:
@@ -386,15 +391,9 @@ class RigidObject(AssetBase):
         # broadcast env_ids if needed to allow double indexing
         if env_ids != slice(None) and body_ids != slice(None):
             env_ids = env_ids[:, None]
-
         # set into internal buffers
         self._external_force_b[env_ids, body_ids] = forces
         self._external_torque_b[env_ids, body_ids] = torques
-        else:
-            self.has_external_wrench = False
-            # reset external wrench
-            self._external_force_b[env_ids] = 0.0
-            self._external_torque_b[env_ids] = 0.0
 
     """
     Internal helper.

source/extensions/omni.isaac.lab/omni/isaac/lab/assets/rigid_object_collection/rigid_object_collection.py

@@ -505,6 +505,11 @@ class RigidObjectCollection(AssetBase):
         """
         if forces.any() or torques.any():
             self.has_external_wrench = True
+        else:
+            self.has_external_wrench = False
+            # to be safe, explicitly set value to zero
+            forces = torques = 0.0
+
         # resolve all indices
         # -- env_ids
         if env_ids is None:
@@ -515,8 +520,6 @@ class RigidObjectCollection(AssetBase):
         # set into internal buffers
         self._external_force_b[env_ids[:, None], object_ids] = forces
         self._external_torque_b[env_ids[:, None], object_ids] = torques
-        else:
-            self.has_external_wrench = False
 
     """
     Internal helper.

source/extensions/omni.isaac.lab/test/assets/test_articulation.py

@@ -621,6 +621,73 @@ class TestArticulation(unittest.TestCase):
                         ) & (articulation._data.default_joint_pos[env_ids][:, joint_ids] <= limits[..., 1])
                         self.assertTrue(torch.all(within_bounds))
 
+    def test_external_force_buffer(self):
+        """Test if external force buffer correctly updates in the force value is zero case."""
+
+        num_articulations = 2
+        for device in ("cuda:0", "cpu"):
+            with self.subTest(num_articulations=num_articulations, device=device):
+                with build_simulation_context(device=device, add_ground_plane=False, auto_add_lighting=True) as sim:
+                    sim._app_control_on_stop_handle = None
+                    articulation_cfg = generate_articulation_cfg(articulation_type="anymal")
+                    articulation, _ = generate_articulation(articulation_cfg, num_articulations, device)
+
+                    # play the simulator
+                    sim.reset()
+
+                    # find bodies to apply the force
+                    body_ids, _ = articulation.find_bodies("base")
+
+                    # reset root state
+                    root_state = articulation.data.default_root_state.clone()
+                    articulation.write_root_state_to_sim(root_state)
+
+                    # reset dof state
+                    joint_pos, joint_vel = (
+                        articulation.data.default_joint_pos,
+                        articulation.data.default_joint_vel,
+                    )
+                    articulation.write_joint_state_to_sim(joint_pos, joint_vel)
+
+                    # reset articulation
+                    articulation.reset()
+
+                    # perform simulation
+                    for step in range(5):
+                        # initiate force tensor
+                        external_wrench_b = torch.zeros(articulation.num_instances, len(body_ids), 6, device=sim.device)
+
+                        if step == 0 or step == 3:
+                            # set a non-zero force
+                            force = 1
+                        else:
+                            # set a zero force
+                            force = 0
+
+                        # set force value
+                        external_wrench_b[:, :, 0] = force
+                        external_wrench_b[:, :, 3] = force
+
+                        # apply force
+                        articulation.set_external_force_and_torque(
+                            external_wrench_b[..., :3], external_wrench_b[..., 3:], body_ids=body_ids
+                        )
+
+                        # check if the articulation's force and torque buffers are correctly updated
+                        for i in range(num_articulations):
+                            self.assertTrue(articulation._external_force_b[i, 0, 0].item() == force)
+                            self.assertTrue(articulation._external_torque_b[i, 0, 0].item() == force)
+
+                        # apply action to the articulation
+                        articulation.set_joint_position_target(articulation.data.default_joint_pos.clone())
+                        articulation.write_data_to_sim()
+
+                        # perform step
+                        sim.step()
+
+                        # update buffers
+                        articulation.update(sim.cfg.dt)
+
     def test_external_force_on_single_body(self):
         """Test application of external force on the base of the articulation."""
         for num_articulations in (1, 2):

source/extensions/omni.isaac.lab/test/assets/test_rigid_object.py

@@ -208,6 +208,64 @@ class TestRigidObject(unittest.TestCase):
                         # Check if object is initialized
                         self.assertFalse(cube_object.is_initialized)
 
+    def test_external_force_buffer(self):
+        """Test if external force buffer correctly updates in the force value is zero case.
+
+        In this test, we apply a non-zero force, then a zero force, then finally a non-zero force
+        to an object. We check if the force buffer is properly updated at each step.
+        """
+
+        for device in ("cuda:0", "cpu"):
+            with self.subTest(num_cubes=1, device=device):
+                # Generate cubes scene
+                with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim:
+                    sim._app_control_on_stop_handle = None
+                    cube_object, origins = generate_cubes_scene(num_cubes=1, device=device)
+
+                    # play the simulator
+                    sim.reset()
+
+                    # find bodies to apply the force
+                    body_ids, body_names = cube_object.find_bodies(".*")
+
+                    # reset object
+                    cube_object.reset()
+
+                    # perform simulation
+                    for step in range(5):
+
+                        # initiate force tensor
+                        external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device)
+
+                        if step == 0 or step == 3:
+                            # set a non-zero force
+                            force = 1
+                        else:
+                            # set a zero force
+                            force = 0
+
+                        # set force value
+                        external_wrench_b[:, :, 0] = force
+                        external_wrench_b[:, :, 3] = force
+
+                        # apply force
+                        cube_object.set_external_force_and_torque(
+                            external_wrench_b[..., :3], external_wrench_b[..., 3:], body_ids=body_ids
+                        )
+
+                        # check if the cube's force and torque buffers are correctly updated
+                        self.assertTrue(cube_object._external_force_b[0, 0, 0].item() == force)
+                        self.assertTrue(cube_object._external_torque_b[0, 0, 0].item() == force)
+
+                        # apply action to the object
+                        cube_object.write_data_to_sim()
+
+                        # perform step
+                        sim.step()
+
+                        # update buffers
+                        cube_object.update(sim.cfg.dt)
+
     def test_external_force_on_single_body(self):
         """Test application of external force on the base of the object.
 

source/extensions/omni.isaac.lab/test/assets/test_rigid_object_collection.py

@@ -228,6 +228,70 @@ class TestRigidObjectCollection(unittest.TestCase):
                         # Check if object is initialized
                         self.assertFalse(object_collection.is_initialized)
 
+    def test_external_force_buffer(self):
+        """Test if external force buffer correctly updates in the force value is zero case.
+
+        In this test, we apply a non-zero force, then a zero force, then finally a non-zero force
+        to an object collection. We check if the force buffer is properly updated at each step.
+        """
+
+        num_envs = 2
+        num_cubes = 1
+        for device in ("cuda:0", "cpu"):
+            with self.subTest(num_cubes=1, device=device):
+                # Generate cubes scene
+                with build_simulation_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim:
+                    sim._app_control_on_stop_handle = None
+                    object_collection, origins = generate_cubes_scene(
+                        num_envs=num_envs, num_cubes=num_cubes, device=device
+                    )
+                    # play the simulator
+                    sim.reset()
+
+                    # find objects to apply the force
+                    object_ids, object_names = object_collection.find_objects(".*")
+
+                    # reset object
+                    object_collection.reset()
+
+                    # perform simulation
+                    for step in range(5):
+
+                        # initiate force tensor
+                        external_wrench_b = torch.zeros(
+                            object_collection.num_instances, len(object_ids), 6, device=sim.device
+                        )
+
+                        if step == 0 or step == 3:
+                            # set a non-zero force
+                            force = 1
+                        else:
+                            # set a zero force
+                            force = 0
+
+                        # set force value
+                        external_wrench_b[:, :, 0] = force
+                        external_wrench_b[:, :, 3] = force
+
+                        # apply force
+                        object_collection.set_external_force_and_torque(
+                            external_wrench_b[..., :3], external_wrench_b[..., 3:], object_ids=object_ids
+                        )
+
+                        # check if the object collection's force and torque buffers are correctly updated
+                        for i in range(num_envs):
+                            self.assertTrue(object_collection._external_force_b[i, 0, 0].item() == force)
+                            self.assertTrue(object_collection._external_torque_b[i, 0, 0].item() == force)
+
+                        # apply action to the object collection
+                        object_collection.write_data_to_sim()
+
+                        # perform step
+                        sim.step()
+
+                        # update buffers
+                        object_collection.update(sim.cfg.dt)
+
     def test_external_force_on_single_body(self):
         """Test application of external force on the base of the object.