Unverified Commit fca3c9ea authored by Antoine RICHARD's avatar Antoine RICHARD Committed by GitHub

Adds `is_global` flag for setting external wrenches on rigid bodies (#3052)

# Description

Added a new argument: is_global to the set_external_force_and_torque
methods of the articulation and rigid body assets. This allows to set
external wrenches in the global frame directly from the method call
rather than having to set the frame in the configuration.

## Type of change

- New feature (non-breaking change which adds functionality)

## Checklist

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

---------
Signed-off-by: 's avatarMayank Mittal <12863862+Mayankm96@users.noreply.github.com>
Co-authored-by: 's avatarMayank Mittal <12863862+Mayankm96@users.noreply.github.com>
parent 0240e825
Changelog
---------
0.41.3 (2025-07-30)
~~~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Added a new argument ``is_global`` to :meth:`~isaaclab.assets.Articulation.set_external_force_and_torque`,
:meth:`~isaaclab.assets.RigidObject.set_external_force_and_torque`, and
:meth:`~isaaclab.assets.RigidObjectCollection.set_external_force_and_torque` allowing to set external wrenches
in the global frame directly from the method call rather than having to set the frame in the configuration.
Removed
^^^^^^^^
* Removed :attr:`xxx_external_wrench_frame` flag from asset configuration classes in favor of direct argument
passed to the :meth:`set_external_force_and_torque` function.
0.41.2 (2025-07-28)
~~~~~~~~~~~~~~~~~~~~
......
......@@ -842,6 +842,7 @@ class Articulation(AssetBase):
positions: torch.Tensor | None = None,
body_ids: Sequence[int] | slice | None = None,
env_ids: Sequence[int] | None = None,
is_global: bool = False,
):
"""Set external force and torque to apply on the asset's bodies in their local frame.
......@@ -859,6 +860,17 @@ class Articulation(AssetBase):
# example of disabling external wrench
asset.set_external_force_and_torque(forces=torch.zeros(0, 3), torques=torch.zeros(0, 3))
.. caution::
If the function is called consecutively with and with different values for ``is_global``, then the
all the external wrenches will be applied in the frame specified by the last call.
.. code-block:: python
# example of setting external wrench in the global frame
asset.set_external_force_and_torque(forces=torch.ones(1, 1, 3), env_ids=[0], is_global=True)
# example of setting external wrench in the link frame
asset.set_external_force_and_torque(forces=torch.ones(1, 1, 3), env_ids=[1], is_global=False)
# Both environments will have the external wrenches applied in the link frame
.. note::
This function does not apply the external wrench to the simulation. It only fills the buffers with
the desired values. To apply the external wrench, call the :meth:`write_data_to_sim` function
......@@ -870,6 +882,8 @@ class Articulation(AssetBase):
positions: Positions to apply external wrench. Shape is (len(env_ids), len(body_ids), 3). Defaults to None.
body_ids: Body indices to apply external wrench to. Defaults to None (all bodies).
env_ids: Environment indices to apply external wrench to. Defaults to None (all instances).
is_global: Whether to apply the external wrench in the global frame. Defaults to False. If set to False,
the external wrench is applied in the link frame of the articulations' bodies.
"""
if forces.any() or torques.any():
self.has_external_wrench = True
......@@ -900,6 +914,13 @@ class Articulation(AssetBase):
self._external_force_b.flatten(0, 1)[indices] = forces.flatten(0, 1)
self._external_torque_b.flatten(0, 1)[indices] = torques.flatten(0, 1)
if is_global != self._use_global_wrench_frame:
omni.log.warn(
f"The external wrench frame has been changed from {self._use_global_wrench_frame} to {is_global}. This"
" may lead to unexpected behavior."
)
self._use_global_wrench_frame = is_global
# If the positions are not provided, the behavior and performance of the simulation should not be affected.
if positions is not None:
# Generates a flag that is set for a full simulation step. This is done to avoid discarding
......@@ -1257,6 +1278,7 @@ class Articulation(AssetBase):
self._external_force_b = torch.zeros((self.num_instances, self.num_bodies, 3), device=self.device)
self._external_torque_b = torch.zeros_like(self._external_force_b)
self._external_wrench_positions_b = torch.zeros_like(self._external_force_b)
self._use_global_wrench_frame = False
# asset named data
self._data.joint_names = self.joint_names
......@@ -1324,15 +1346,6 @@ class Articulation(AssetBase):
default_root_state = torch.tensor(default_root_state, dtype=torch.float, device=self.device)
self._data.default_root_state = default_root_state.repeat(self.num_instances, 1)
# -- external wrench
external_wrench_frame = self.cfg.articulation_external_wrench_frame
if external_wrench_frame == "local":
self._use_global_wrench_frame = False
elif external_wrench_frame == "world":
self._use_global_wrench_frame = True
else:
raise ValueError(f"Invalid external wrench frame: {external_wrench_frame}. Must be 'local' or 'world'.")
# -- joint state
self._data.default_joint_pos = torch.zeros(self.num_instances, self.num_joints, device=self.device)
self._data.default_joint_vel = torch.zeros_like(self._data.default_joint_pos)
......
......@@ -44,13 +44,6 @@ class ArticulationCfg(AssetBaseCfg):
If not provided will search for a prim with the ArticulationRootAPI. Should start with a slash.
"""
articulation_external_wrench_frame: str = "local"
"""Frame in which external wrenches are applied. Defaults to "local".
If "local", the external wrenches are applied in the local frame of the articulation root.
If "world", the external wrenches are applied in the world frame.
"""
init_state: InitialStateCfg = InitialStateCfg()
"""Initial state of the articulated object. Defaults to identity pose with zero velocity and zero joint state."""
......
......@@ -370,6 +370,7 @@ class RigidObject(AssetBase):
positions: torch.Tensor | None = None,
body_ids: Sequence[int] | slice | None = None,
env_ids: Sequence[int] | None = None,
is_global: bool = False,
):
"""Set external force and torque to apply on the asset's bodies in their local frame.
......@@ -387,6 +388,17 @@ class RigidObject(AssetBase):
# example of disabling external wrench
asset.set_external_force_and_torque(forces=torch.zeros(0, 3), torques=torch.zeros(0, 3))
.. caution::
If the function is called consecutively with and with different values for ``is_global``, then the
all the external wrenches will be applied in the frame specified by the last call.
.. code-block:: python
# example of setting external wrench in the global frame
asset.set_external_force_and_torque(forces=torch.ones(1, 1, 3), env_ids=[0], is_global=True)
# example of setting external wrench in the link frame
asset.set_external_force_and_torque(forces=torch.ones(1, 1, 3), env_ids=[1], is_global=False)
# Both environments will have the external wrenches applied in the link frame
.. note::
This function does not apply the external wrench to the simulation. It only fills the buffers with
the desired values. To apply the external wrench, call the :meth:`write_data_to_sim` function
......@@ -398,6 +410,8 @@ class RigidObject(AssetBase):
positions: External wrench positions in bodies' local frame. Shape is (len(env_ids), len(body_ids), 3). Defaults to None.
body_ids: Body indices to apply external wrench to. Defaults to None (all bodies).
env_ids: Environment indices to apply external wrench to. Defaults to None (all instances).
is_global: Whether to apply the external wrench in the global frame. Defaults to False. If set to False,
the external wrench is applied in the link frame of the bodies.
"""
if forces.any() or torques.any():
self.has_external_wrench = True
......@@ -420,6 +434,13 @@ class RigidObject(AssetBase):
self._external_force_b[env_ids, body_ids] = forces
self._external_torque_b[env_ids, body_ids] = torques
if is_global != self._use_global_wrench_frame:
omni.log.warn(
f"The external wrench frame has been changed from {self._use_global_wrench_frame} to {is_global}. This"
" may lead to unexpected behavior."
)
self._use_global_wrench_frame = is_global
if positions is not None:
self.uses_external_wrench_positions = True
self._external_wrench_positions_b[env_ids, body_ids] = positions
......@@ -505,6 +526,7 @@ class RigidObject(AssetBase):
self._external_torque_b = torch.zeros_like(self._external_force_b)
self.uses_external_wrench_positions = False
self._external_wrench_positions_b = torch.zeros_like(self._external_force_b)
self._use_global_wrench_frame = False
# set information about rigid body into data
self._data.body_names = self.body_names
......@@ -525,15 +547,6 @@ class RigidObject(AssetBase):
default_root_state = torch.tensor(default_root_state, dtype=torch.float, device=self.device)
self._data.default_root_state = default_root_state.repeat(self.num_instances, 1)
# -- external wrench
external_wrench_frame = self.cfg.object_external_wrench_frame
if external_wrench_frame == "local":
self._use_global_wrench_frame = False
elif external_wrench_frame == "world":
self._use_global_wrench_frame = True
else:
raise ValueError(f"Invalid external wrench frame: {external_wrench_frame}. Must be 'local' or 'world'.")
"""
Internal simulation callbacks.
"""
......
......@@ -30,10 +30,3 @@ class RigidObjectCfg(AssetBaseCfg):
init_state: InitialStateCfg = InitialStateCfg()
"""Initial state of the rigid object. Defaults to identity pose with zero velocity."""
object_external_wrench_frame: str = "local"
"""Frame in which external wrenches are applied. Defaults to "local".
If "local", the external wrenches are applied in the local frame of the articulation root.
If "world", the external wrenches are applied in the world frame.
"""
......@@ -499,6 +499,7 @@ class RigidObjectCollection(AssetBase):
positions: torch.Tensor | None = None,
object_ids: slice | torch.Tensor | None = None,
env_ids: torch.Tensor | None = None,
is_global: bool = False,
):
"""Set external force and torque to apply on the objects' bodies in their local frame.
......@@ -515,6 +516,17 @@ class RigidObjectCollection(AssetBase):
# example of disabling external wrench
asset.set_external_force_and_torque(forces=torch.zeros(0, 0, 3), torques=torch.zeros(0, 0, 3))
.. caution::
If the function is called consecutively with and with different values for ``is_global``, then the
all the external wrenches will be applied in the frame specified by the last call.
.. code-block:: python
# example of setting external wrench in the global frame
asset.set_external_force_and_torque(forces=torch.ones(1, 1, 3), env_ids=[0], is_global=True)
# example of setting external wrench in the link frame
asset.set_external_force_and_torque(forces=torch.ones(1, 1, 3), env_ids=[1], is_global=False)
# Both environments will have the external wrenches applied in the link frame
.. note::
This function does not apply the external wrench to the simulation. It only fills the buffers with
the desired values. To apply the external wrench, call the :meth:`write_data_to_sim` function
......@@ -526,6 +538,8 @@ class RigidObjectCollection(AssetBase):
positions: External wrench positions in bodies' local frame. Shape is (len(env_ids), len(object_ids), 3).
object_ids: Object indices to apply external wrench to. Defaults to None (all objects).
env_ids: Environment indices to apply external wrench to. Defaults to None (all instances).
is_global: Whether to apply the external wrench in the global frame. Defaults to False. If set to False,
the external wrench is applied in the link frame of the bodies.
"""
if forces.any() or torques.any():
self.has_external_wrench = True
......@@ -544,6 +558,14 @@ 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
if is_global != self._use_global_wrench_frame:
omni.log.warn(
f"The external wrench frame has been changed from {self._use_global_wrench_frame} to {is_global}. This"
" may lead to unexpected behavior."
)
self._use_global_wrench_frame = is_global
if positions is not None:
self.uses_external_wrench_positions = True
self._external_wrench_positions_b[env_ids[:, None], object_ids] = positions
......@@ -663,6 +685,7 @@ class RigidObjectCollection(AssetBase):
self._external_torque_b = torch.zeros_like(self._external_force_b)
self._external_wrench_positions_b = torch.zeros_like(self._external_force_b)
self.uses_external_wrench_positions = False
self._use_global_wrench_frame = False
# set information about rigid body into data
self._data.object_names = self.object_names
......@@ -691,15 +714,6 @@ class RigidObjectCollection(AssetBase):
default_object_states = torch.cat(default_object_states, dim=1)
self._data.default_object_state = default_object_states
# -- external wrench
external_wrench_frame = self.cfg.objects_external_wrench_frame
if external_wrench_frame == "local":
self._use_global_wrench_frame = False
elif external_wrench_frame == "world":
self._use_global_wrench_frame = True
else:
raise ValueError(f"Invalid external wrench frame: {external_wrench_frame}. Must be 'local' or 'world'.")
def _env_obj_ids_to_view_ids(
self, env_ids: torch.Tensor, object_ids: Sequence[int] | slice | torch.Tensor
) -> torch.Tensor:
......
......@@ -26,10 +26,3 @@ class RigidObjectCollectionCfg:
The keys are the names for the objects, which are used as unique identifiers throughout the code.
"""
objects_external_wrench_frame: str = "local"
"""Frame in which external wrenches are applied. Defaults to "local".
If "local", the external wrenches are applied in the local frame of the articulation root.
If "world", the external wrenches are applied in the world frame.
"""
......@@ -775,10 +775,14 @@ def test_external_force_buffer(sim, num_articulations, device):
external_wrench_b[..., 3:],
body_ids=body_ids,
positions=external_wrench_positions_b,
is_global=True,
)
else:
articulation.set_external_force_and_torque(
external_wrench_b[..., :3], external_wrench_b[..., 3:], body_ids=body_ids
external_wrench_b[..., :3],
external_wrench_b[..., 3:],
body_ids=body_ids,
is_global=False,
)
# check if the articulation's force and torque buffers are correctly updated
......@@ -786,6 +790,7 @@ def test_external_force_buffer(sim, num_articulations, device):
assert articulation._external_force_b[i, 0, 0].item() == force
assert articulation._external_torque_b[i, 0, 0].item() == force
assert articulation._external_wrench_positions_b[i, 0, 0].item() == position
assert articulation._use_global_wrench_frame == (step == 0 or step == 3)
# apply action to the articulation
articulation.set_joint_position_target(articulation.data.default_joint_pos.clone())
......
......@@ -230,10 +230,12 @@ def test_external_force_buffer(device):
# set a non-zero force
force = 1
position = 1
is_global = True
else:
# set a zero force
force = 0
position = 0
is_global = False
# set force value
external_wrench_b[:, :, 0] = force
......@@ -247,16 +249,21 @@ def test_external_force_buffer(device):
external_wrench_b[..., 3:],
body_ids=body_ids,
positions=external_wrench_positions_b,
is_global=is_global,
)
else:
cube_object.set_external_force_and_torque(
external_wrench_b[..., :3], external_wrench_b[..., 3:], body_ids=body_ids
external_wrench_b[..., :3],
external_wrench_b[..., 3:],
body_ids=body_ids,
is_global=is_global,
)
# check if the cube's force and torque buffers are correctly updated
assert cube_object._external_force_b[0, 0, 0].item() == force
assert cube_object._external_torque_b[0, 0, 0].item() == force
assert cube_object._external_wrench_positions_b[0, 0, 0].item() == position
assert cube_object._use_global_wrench_frame == (step == 0 or step == 3)
# apply action to the object
cube_object.write_data_to_sim()
......
......@@ -241,9 +241,11 @@ def test_external_force_buffer(sim, device):
if step == 0 or step == 3:
force = 1.0
position = 1.0
is_global = True
else:
force = 0.0
position = 0.0
is_global = False
# apply force to the object
external_wrench_b[:, :, 0] = force
......@@ -255,6 +257,7 @@ def test_external_force_buffer(sim, device):
external_wrench_b[..., 3:],
object_ids=object_ids,
positions=external_wrench_positions_b,
is_global=is_global,
)
# check if the object collection's force and torque buffers are correctly updated
......@@ -262,6 +265,7 @@ def test_external_force_buffer(sim, device):
assert object_collection._external_force_b[i, 0, 0].item() == force
assert object_collection._external_torque_b[i, 0, 0].item() == force
assert object_collection._external_wrench_positions_b[i, 0, 0].item() == position
assert object_collection._use_global_wrench_frame == (step == 0 or step == 3)
# apply action to the object collection
object_collection.write_data_to_sim()
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment