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
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.
Describe the bug
When sending wrenches via
RigidObject.set_external_force_and_torque(), sending zeros for both forces and torques sets theRigidObject.has_external_wrenchflag to false. I assume this is done to save computation inRigidObject.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_bstill contains the last value that was realized fromRigidObject.write_data_to_sim().Steps to reproduce
Consider a randomization term similar to
apply_external_force_torquebut with the desired wrench set via a function, e.g.When both forces and torques are zero,
RigidObject.has_external_wrenchwill be set to false and whatever the preceding value sent toRigidObject._external_force_bwas will get stuck in the buffer until set to a non-zero value.Potential fix
Clear the buffers when
RigidObject.has_external_wrenchgets set to False:System Info
Describe the characteristic of your environment:
Additional context
Add any other context about the problem here.
Checklist
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.
RigidObject._external_force_btensor whenRigidObject.set_external_force_and_torque()is sent zeros