[Bug Report] RigidObject.set_external_force_and_torque() doesn't clear buffers

[kmorgenstein-bdai]

Describe the bug

When sending wrenches via RigidObject.set_external_force_and_torque(), sending zeros for both forces and torques sets the RigidObject.has_external_wrench flag to false. I assume this is done to save computation in RigidObject.write_data_to_sim() so the simulation only sets non zero forces. However, when the flag goes from True to False, RigidObject.write_data_to_sim() does not clear the external force buffer, and as a result, the last value sent to the external force buffer gets stuck in the buffer. Even though no force is being actuated, RigidObject._external_force_b still contains the last value that was realized from RigidObject.write_data_to_sim().

Steps to reproduce

Consider a randomization term similar to apply_external_force_torque but with the desired wrench set via a function, e.g.

forces = torch.sin(env.common_step_counter) * torch.ones(num_envs, num_bodies, 3)
torques = torch.sin(env.common_step_counter) * torch.ones(num_envs, num_bodies, 3)
asset.set_external_force_and_torque(forces, torques, env_ids=env_ids, body_ids=asset_cfg.body_ids)

When both forces and torques are zero, RigidObject.has_external_wrench will be set to false and whatever the preceding value sent to RigidObject._external_force_b was will get stuck in the buffer until set to a non-zero value.

Potential fix

Clear the buffers when RigidObject.has_external_wrench gets set to False:

def set_external_force_and_torque(
        self,
        forces: torch.Tensor,
        torques: torch.Tensor,
        body_ids: Sequence[int] | None = None,
        env_ids: Sequence[int] | None = None,
    ):
        """
        Docstring
        """
        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:
            env_ids = self._ALL_INDICES
         elif not isinstance(env_ids, torch.Tensor):
             env_ids = torch.tensor(env_ids, dtype=torch.long, device=self.device)
         # -- body_ids
         if body_ids is None:
             body_ids = torch.arange(self.num_bodies, dtype=torch.long, device=self.device)
         elif not isinstance(body_ids, torch.Tensor):
             body_ids = torch.tensor(body_ids, dtype=torch.long, device=self.device)

         # note: we need to do this complicated indexing since torch doesn't support multi-indexing
         # create global body indices from env_ids and env_body_ids
         # (env_id * total_bodies_per_env) + body_id
         total_bodies_per_env = self.body_physx_view.count // self.root_physx_view.count
         indices = body_ids.repeat(len(env_ids), 1) + env_ids.unsqueeze(1) * total_bodies_per_env
         indices = indices.view(-1)
         # set into internal buffers
         # 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)

System Info

Describe the characteristic of your environment:

• Commit: main
• Isaac Sim Version:2023.1.1, 2023.1.0.hotfix
• OS: Ubuntu 20.04
• GPU: L4

Additional context

Add any other context about the problem here.

Checklist

• [X] I have checked that there is no similar issue in the repo (required)
• [X] I have checked that the issue is not in running Isaac Sim itself and is related to the repo

Acceptance Criteria

Add the criteria for which this task is considered done. If not known at issue creation time, you can add this once the issue is assigned.

• [ ] Have a way to clear the RigidObject._external_force_b tensor when RigidObject.set_external_force_and_torque() is sent zeros

[hhansen-bdai]

Thank you, @kmorgenstein-bdai! I love issues that come with a proposed fix :smile:. I will take a look at this as soon as I can