Automates finding of root prims for the asset classes (#250)

# Description

Previously, the `Asset` classes assumed that the root prim existed at
the spawn prim location. This means that when the robot is spawned at
`/World/Robot`, the articulation root prim is also at `/World/Root`.
However, this is not safe operation as many assets will have the root
defined under the spawn location.

The MR safely checks if there are prims under the spawn location that
have the API defined for them. If they do, then it uses that prim path
for initializing the views.

## 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
`./orbit.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

source/extensions/omni.isaac.orbit/config/extension.toml

 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.9.46"
+version = "0.9.47"
 
 # Description
 title = "ORBIT framework for Robot Learning"

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

 Changelog
 ---------
 
+0.9.47 (2023-11-24)
+~~~~~~~~~~~~~~~~~~~
+
+Fixed
+^^^^^
+
+* Automated identification of the root prim in the :class:`omni.isaac.orbit.assets.RigidObject` and
+  :class:`omni.isaac.orbit.assets.Articulation` classes. Earlier, the root prim was hard-coded to
+  the spawn prim path. Now, the class searches for the root prim under the spawn prim path.
+
+
 0.9.46 (2023-11-24)
 ~~~~~~~~~~~~~~~~~~~
 

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

@@ -12,6 +12,7 @@ import torch
 from typing import TYPE_CHECKING, Sequence
 
 import carb
+import omni.isaac.core.utils.prims as prim_utils
 import omni.physics.tensors.impl.api as physx
 from omni.isaac.core.articulations import ArticulationView
 from omni.isaac.core.prims import RigidPrimView
@@ -357,8 +358,21 @@ class Articulation(RigidObject):
     """
 
     def _initialize_impl(self):
+        # find articulation root prims
+        asset_prim_path = prim_utils.find_matching_prim_paths(self.cfg.prim_path)[0]
+        root_prims = prim_utils.get_all_matching_child_prims(
+            asset_prim_path, predicate=lambda a: prim_utils.get_prim_at_path(a).HasAPI(UsdPhysics.ArticulationRootAPI)
+        )
+        if len(root_prims) != 1:
+            raise RuntimeError(
+                f"Failed to find a single articulation root when resolving '{self.cfg.prim_path}'."
+                f" Found roots '{root_prims}' under '{asset_prim_path}'."
+            )
+        # resolve articulation root prim back into regex expression
+        root_prim_path = prim_utils.get_prim_path(root_prims[0])
+        root_prim_path_expr = self.cfg.prim_path + root_prim_path[len(asset_prim_path) :]
         # -- articulation
-        self._root_view = ArticulationView(self.cfg.prim_path, reset_xform_properties=False)
+        self._root_view = ArticulationView(root_prim_path_expr, reset_xform_properties=False)
         # Hacking the initialization of the articulation view.
         # reason: The default initialization of the articulation view is not working properly as it tries to create
         # default actions that is not possible within the post-play callback.
@@ -395,8 +409,7 @@ class Articulation(RigidObject):
                     self._is_fixed_base = True
                     break
         # log information about the articulation
-        carb.log_info(f"Articulation initialized at: {self.cfg.prim_path}")
-        carb.log_info(f"Root name: {self.body_names[0]}")
+        carb.log_info(f"Articulation initialized at: {self.cfg.prim_path} with root '{root_prim_path_expr}'.")
         carb.log_info(f"Is fixed root: {self.is_fixed_base}")
         carb.log_info(f"Number of bodies: {self.num_bodies}")
         carb.log_info(f"Body names: {self.body_names}")

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

@@ -9,8 +9,10 @@ import torch
 from typing import TYPE_CHECKING, Sequence
 
 import carb
+import omni.isaac.core.utils.prims as prim_utils
 import omni.physics.tensors.impl.api as physx
 from omni.isaac.core.prims import RigidPrimView
+from pxr import UsdPhysics
 
 import omni.isaac.orbit.utils.math as math_utils
 import omni.isaac.orbit.utils.string as string_utils
@@ -266,11 +268,24 @@ class RigidObject(AssetBase):
     """
 
     def _initialize_impl(self):
+        # find articulation root prims
+        asset_prim_path = prim_utils.find_matching_prim_paths(self.cfg.prim_path)[0]
+        root_prims = prim_utils.get_all_matching_child_prims(
+            asset_prim_path, predicate=lambda a: prim_utils.get_prim_at_path(a).HasAPI(UsdPhysics.RigidBodyAPI)
+        )
+        if len(root_prims) != 1:
+            raise RuntimeError(
+                f"Failed to find a single rigid body when resolving '{self.cfg.prim_path}'."
+                f" Found multiple '{root_prims}' under '{asset_prim_path}'."
+            )
+        # resolve articulation root prim back into regex expression
+        root_prim_path = prim_utils.get_prim_path(root_prims[0])
+        root_prim_path_expr = self.cfg.prim_path + root_prim_path[len(asset_prim_path) :]
         # -- object views
-        self._root_view = RigidPrimView(self.cfg.prim_path, reset_xform_properties=False)
+        self._root_view = RigidPrimView(root_prim_path_expr, reset_xform_properties=False)
         self._root_view.initialize()
         # log information about the articulation
-        carb.log_info(f"Rigid body initialized at: {self.cfg.prim_path}")
+        carb.log_info(f"Rigid body initialized at: {self.cfg.prim_path} with root '{root_prim_path_expr}'.")
         carb.log_info(f"Number of bodies (orbit): {self.num_bodies}")
         carb.log_info(f"Body names (orbit): {self.body_names}")
         # create buffers

source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/schemas.py

@@ -127,15 +127,15 @@ def define_rigid_body_properties(
     # obtain stage
     if stage is None:
         stage = stage_utils.get_current_stage()
-    # get articulation USD prim
+    # get USD prim
     prim = stage.GetPrimAtPath(prim_path)
     # check if prim path is valid
     if not prim.IsValid():
         raise ValueError(f"Prim path '{prim_path}' is not valid.")
-    # check if prim has articulation applied on it
+    # check if prim has rigid body applied on it
     if not UsdPhysics.RigidBodyAPI(prim):
         UsdPhysics.RigidBodyAPI.Apply(prim)
-    # set articulation root properties
+    # set rigid body properties
     modify_rigid_body_properties(prim_path, cfg, stage)
 
 
@@ -219,15 +219,15 @@ def define_collision_properties(
     # obtain stage
     if stage is None:
         stage = stage_utils.get_current_stage()
-    # get articulation USD prim
+    # get USD prim
     prim = stage.GetPrimAtPath(prim_path)
     # check if prim path is valid
     if not prim.IsValid():
         raise ValueError(f"Prim path '{prim_path}' is not valid.")
-    # check if prim has articulation applied on it
+    # check if prim has collision applied on it
     if not UsdPhysics.CollisionAPI(prim):
         UsdPhysics.CollisionAPI.Apply(prim)
-    # set articulation root properties
+    # set collision properties
     modify_collision_properties(prim_path, cfg, stage)
 
 
@@ -265,7 +265,7 @@ def modify_collision_properties(
     # check if prim has collision applied on it
     if not UsdPhysics.CollisionAPI(collider_prim):
         return False
-    # retrieve the USD rigid-body api
+    # retrieve the USD collision api
     usd_collision_api = UsdPhysics.CollisionAPI(collider_prim)
     # retrieve the collision api
     physx_collision_api = PhysxSchema.PhysxCollisionAPI(collider_prim)
@@ -307,15 +307,15 @@ def define_mass_properties(prim_path: str, cfg: schemas_cfg.MassPropertiesCfg, s
     # obtain stage
     if stage is None:
         stage = stage_utils.get_current_stage()
-    # get articulation USD prim
+    # get USD prim
     prim = stage.GetPrimAtPath(prim_path)
     # check if prim path is valid
     if not prim.IsValid():
         raise ValueError(f"Prim path '{prim_path}' is not valid.")
-    # check if prim has articulation applied on it
+    # check if prim has mass applied on it
     if not UsdPhysics.MassAPI(prim):
         UsdPhysics.MassAPI.Apply(prim)
-    # set articulation root properties
+    # set mass properties
     modify_mass_properties(prim_path, cfg, stage)
 
 
@@ -351,10 +351,10 @@ def modify_mass_properties(prim_path: str, cfg: schemas_cfg.MassPropertiesCfg, s
         stage = stage_utils.get_current_stage()
     # get USD prim
     rigid_prim = stage.GetPrimAtPath(prim_path)
-    # check if prim has collision applied on it
+    # check if prim has mass API applied on it
     if not UsdPhysics.MassAPI(rigid_prim):
         return False
-    # retrieve the USD rigid-body api
+    # retrieve the USD mass api
     usd_physics_mass_api = UsdPhysics.MassAPI(rigid_prim)
 
     # convert to dict

source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/spawners/from_files/from_files.py

@@ -57,43 +57,8 @@ def spawn_from_usd(
     Raises:
         FileNotFoundError: If the USD file does not exist at the given path.
     """
-    # check file path exists
-    if not check_file_path(cfg.usd_path):
-        raise FileNotFoundError(f"USD file not found at path: '{cfg.usd_path}'.")
-    # spawn asset if it doesn't exist.
-    if not prim_utils.is_prim_path_valid(prim_path):
-        # add prim as reference to stage
-        prim_utils.create_prim(
-            prim_path,
-            usd_path=cfg.usd_path,
-            translation=translation,
-            orientation=orientation,
-            scale=cfg.scale,
-        )
-    else:
-        carb.log_warn(f"A prim already exists at prim path: '{prim_path}'.")
-
-    # modify rigid body properties
-    if cfg.rigid_props is not None:
-        schemas.modify_rigid_body_properties(prim_path, cfg.rigid_props)
-    # modify collision properties
-    if cfg.collision_props is not None:
-        schemas.modify_collision_properties(prim_path, cfg.collision_props)
-    # modify articulation root properties
-    if cfg.articulation_props is not None:
-        schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props)
-    # apply visual material
-    if cfg.visual_material is not None:
-        if not cfg.visual_material_path.startswith("/"):
-            material_path = f"{prim_path}/{cfg.visual_material_path}"
-        else:
-            material_path = cfg.visual_material_path
-        # create material
-        cfg.visual_material.func(material_path, cfg.visual_material)
-        # apply material
-        bind_visual_material(prim_path, material_path)
-    # return the prim
-    return prim_utils.get_prim_at_path(prim_path)
+    # spawn asset from the given usd file
+    return _spawn_from_usd_file(prim_path, cfg.usd_path, cfg, translation, orientation)
 
 
 @clone
@@ -132,42 +97,10 @@ def spawn_from_urdf(
     Raises:
         FileNotFoundError: If the URDF file does not exist at the given path.
     """
-    # spawn asset if it doesn't exist.
-    if not prim_utils.is_prim_path_valid(prim_path):
-        # urdf loader
-        urdf_loader = converters.UrdfConverter(cfg)
-        # add prim as reference to stage
-        prim_utils.create_prim(
-            prim_path,
-            usd_path=urdf_loader.usd_path,
-            translation=translation,
-            orientation=orientation,
-            scale=cfg.scale,
-        )
-    else:
-        carb.log_warn(f"A prim already exists at prim path: '{prim_path}'. Skipping...")
-
-    # modify rigid body properties
-    if cfg.rigid_props is not None:
-        schemas.modify_rigid_body_properties(prim_path, cfg.rigid_props)
-    # modify collision properties
-    if cfg.collision_props is not None:
-        schemas.modify_collision_properties(prim_path, cfg.collision_props)
-    # modify articulation root properties
-    if cfg.articulation_props is not None:
-        schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props)
-    # apply visual material
-    if cfg.visual_material is not None:
-        if not cfg.visual_material_path.startswith("/"):
-            material_path = f"{prim_path}/{cfg.visual_material_path}"
-        else:
-            material_path = cfg.visual_material_path
-        # create material
-        cfg.visual_material.func(material_path, cfg.visual_material)
-        # apply material
-        bind_visual_material(prim_path, material_path)
-    # return the prim
-    return prim_utils.get_prim_at_path(prim_path)
+    # urdf loader to convert urdf to usd
+    urdf_loader = converters.UrdfConverter(cfg)
+    # spawn asset from the generated usd file
+    return _spawn_from_usd_file(prim_path, urdf_loader.usd_path, cfg, translation, orientation)
 
 
 def spawn_ground_plane(
@@ -254,3 +187,80 @@ def spawn_ground_plane(
 
     # return the prim
     return prim_utils.get_prim_at_path(prim_path)
+
+
+"""
+Helper functions.
+"""
+
+
+def _spawn_from_usd_file(
+    prim_path: str,
+    usd_path: str,
+    cfg: from_files_cfg.FileCfg,
+    translation: tuple[float, float, float] | None = None,
+    orientation: tuple[float, float, float, float] | None = None,
+) -> Usd.Prim:
+    """Spawn an asset from a USD file and override the settings with the given config.
+
+    In case a prim already exists at the given prim path, then the function does not create a new prim
+    or throw an error that the prim already exists. Instead, it just takes the existing prim and overrides
+    the settings with the given config.
+
+    Args:
+        prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern,
+            then the asset is spawned at all the matching prim paths.
+        cfg: The configuration instance.
+        translation: The translation to apply to the prim w.r.t. its parent prim. Defaults to None, in which
+            case the translation specified in the generated USD file is used.
+        orientation: The orientation in (w, x, y, z) to apply to the prim w.r.t. its parent prim. Defaults to None,
+            in which case the orientation specified in the generated USD file is used.
+
+    Returns:
+        The prim of the spawned asset.
+
+    Raises:
+        FileNotFoundError: If the USD file does not exist at the given path.
+    """
+    # check file path exists
+    if not check_file_path(usd_path):
+        raise FileNotFoundError(f"USD file not found at path: '{usd_path}'.")
+    # spawn asset if it doesn't exist.
+    if not prim_utils.is_prim_path_valid(prim_path):
+        # add prim as reference to stage
+        prim_utils.create_prim(
+            prim_path,
+            usd_path=usd_path,
+            translation=translation,
+            orientation=orientation,
+            scale=cfg.scale,
+        )
+    else:
+        carb.log_warn(f"A prim already exists at prim path: '{prim_path}'.")
+
+    # modify rigid body properties
+    if cfg.rigid_props is not None:
+        schemas.modify_rigid_body_properties(prim_path, cfg.rigid_props)
+    # modify collision properties
+    if cfg.collision_props is not None:
+        schemas.modify_collision_properties(prim_path, cfg.collision_props)
+    # modify mass properties
+    if cfg.mass_props is not None:
+        schemas.modify_mass_properties(prim_path, cfg.mass_props)
+    # modify articulation root properties
+    if cfg.articulation_props is not None:
+        schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props)
+
+    # apply visual material
+    if cfg.visual_material is not None:
+        if not cfg.visual_material_path.startswith("/"):
+            material_path = f"{prim_path}/{cfg.visual_material_path}"
+        else:
+            material_path = cfg.visual_material_path
+        # create material
+        cfg.visual_material.func(material_path, cfg.visual_material)
+        # apply material
+        bind_visual_material(prim_path, material_path)
+
+    # return the prim
+    return prim_utils.get_prim_at_path(prim_path)

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

@@ -27,8 +27,10 @@ import carb
 import omni.isaac.core.utils.stage as stage_utils
 
 import omni.isaac.orbit.sim as sim_utils
-from omni.isaac.orbit.assets import Articulation
+from omni.isaac.orbit.actuators import ImplicitActuatorCfg
+from omni.isaac.orbit.assets import Articulation, ArticulationCfg
 from omni.isaac.orbit.assets.config import ANYMAL_C_CFG, FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
+from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR
 
 
 class TestArticulation(unittest.TestCase):
@@ -55,8 +57,41 @@ class TestArticulation(unittest.TestCase):
     Tests
     """
 
+    def test_initialization_floating_base_non_root(self):
+        """Test initialization for a floating-base with articulation root on a rigid body
+        under the provided prim path."""
+        # Create articulation
+        robot_cfg = ArticulationCfg(
+            prim_path="/World/Robot",
+            spawn=sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Humanoid/humanoid_instanceable.usd"),
+            init_state=ArticulationCfg.InitialStateCfg(pos=(0.0, 0.0, 1.34)),
+            actuators={"body": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=0.0, damping=0.0)},
+        )
+        robot = Articulation(cfg=robot_cfg)
+
+        # Check that boundedness of articulation is correct
+        self.assertEqual(ctypes.c_long.from_address(id(robot)).value, 1)
+
+        # Play sim
+        self.sim.reset()
+        # Check if robot is initialized
+        self.assertTrue(robot._is_initialized)
+        # Check that floating base
+        self.assertFalse(robot.is_fixed_base)
+        # Check buffers that exists and have correct shapes
+        self.assertTrue(robot.data.root_pos_w.shape == (1, 3))
+        self.assertTrue(robot.data.root_quat_w.shape == (1, 4))
+        self.assertTrue(robot.data.joint_pos.shape == (1, 21))
+
+        # Simulate physics
+        for _ in range(10):
+            # perform rendering
+            self.sim.step()
+            # update robot
+            robot.update(self.dt)
+
     def test_initialization_floating_base(self):
-        """Test articulation initialization for a floating-base."""
+        """Test initialization for a floating-base with articulation root on provided prim path."""
         # Create articulation
         robot = Articulation(cfg=ANYMAL_C_CFG.replace(prim_path="/World/Robot"))
 
@@ -82,7 +117,7 @@ class TestArticulation(unittest.TestCase):
             robot.update(self.dt)
 
     def test_initialization_fixed_base(self):
-        """Test articulation initialization for fixed base."""
+        """Test initialization for fixed base."""
         # Create articulation
         robot = Articulation(cfg=FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG.replace(prim_path="/World/Robot"))
 
@@ -136,6 +171,7 @@ class TestArticulation(unittest.TestCase):
             # reset dof state
             joint_pos, joint_vel = robot.data.default_joint_pos, robot.data.default_joint_vel
             robot.write_joint_state_to_sim(joint_pos, joint_vel)
+            # reset robot
             robot.reset()
             # apply force
             robot.set_external_force_and_torque(
@@ -183,6 +219,7 @@ class TestArticulation(unittest.TestCase):
             # reset dof state
             joint_pos, joint_vel = robot.data.default_joint_pos, robot.data.default_joint_vel
             robot.write_joint_state_to_sim(joint_pos, joint_vel)
+            # reset robot
             robot.reset()
             # apply force
             robot.set_external_force_and_torque(

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

+# Copyright (c) 2022-2023, The ORBIT Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+# ignore private usage of variables warning
+# pyright: reportPrivateUsage=none
+
+from __future__ import annotations
+
+"""Launch Isaac Sim Simulator first."""
+
+from omni.isaac.orbit.app import AppLauncher
+
+# launch omniverse app
+app_launcher = AppLauncher(headless=True)
+simulation_app = app_launcher.app
+
+"""Rest everything follows."""
+
+import ctypes
+import torch
+import traceback
+import unittest
+
+import carb
+import omni.isaac.core.utils.stage as stage_utils
+import omni.isaac.core.utils.prims as prim_utils
+
+import omni.isaac.orbit.sim as sim_utils
+from omni.isaac.orbit.assets import RigidObject, RigidObjectCfg
+from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR
+
+
+class TestRigidObject(unittest.TestCase):
+    """Test for rigid object class."""
+
+    def setUp(self):
+        """Create a blank new stage for each test."""
+        # Create a new stage
+        stage_utils.create_new_stage()
+        # Simulation time-step
+        self.dt = 0.01
+        # Load kit helper
+        sim_cfg = sim_utils.SimulationCfg(dt=self.dt, device="cuda:0", shutdown_app_on_stop=False)
+        self.sim = sim_utils.SimulationContext(sim_cfg)
+
+    def tearDown(self):
+        """Stops simulator after each test."""
+        # stop simulation
+        self.sim.stop()
+        # clear the stage
+        self.sim.clear_instance()
+
+    """
+    Tests
+    """
+
+    def test_initialization(self):
+        """Test initialization for with rigid body API at the provided prim path."""
+        # Create rigid object
+        cube_object_cfg = RigidObjectCfg(
+            prim_path="/World/Object",
+            spawn=sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd"),
+            init_state=RigidObjectCfg.InitialStateCfg(),
+        )
+        cube_object = RigidObject(cfg=cube_object_cfg)
+
+        # Check that boundedness of articulation is correct
+        self.assertEqual(ctypes.c_long.from_address(id(cube_object)).value, 1)
+
+        # Play sim
+        self.sim.reset()
+        # Check if object is initialized
+        self.assertTrue(cube_object._is_initialized)
+        self.assertEqual(len(cube_object.body_names), 1)
+        # Check buffers that exists and have correct shapes
+        self.assertTrue(cube_object.data.root_pos_w.shape == (1, 3))
+        self.assertTrue(cube_object.data.root_quat_w.shape == (1, 4))
+
+        # Simulate physics
+        for _ in range(20):
+            # perform rendering
+            self.sim.step()
+            # update object
+            cube_object.update(self.dt)
+
+    def test_external_force_on_single_body(self):
+        """Test application of external force on the base of the object.
+
+        In this test, we apply a force equal to the weight of the object on the base of
+        one of the objects. We check that the object does not move. For the other object,
+        we do not apply any force and check that it falls down.
+        """
+        # Create parent prims
+        prim_utils.create_prim("/World/Table_1", "Xform", translation=(0.0, -1.0, 0.0))
+        prim_utils.create_prim("/World/Table_2", "Xform", translation=(0.0, 1.0, 0.0))
+
+        # Create rigid object
+        cube_object_cfg = RigidObjectCfg(
+            prim_path="/World/Table_.*/Object",
+            spawn=sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd"),
+            init_state=RigidObjectCfg.InitialStateCfg(),
+        )
+        # create handles for the objects
+        cube_object = RigidObject(cfg=cube_object_cfg)
+
+        # Play the simulator
+        self.sim.reset()
+
+        # Find bodies to apply the force
+        body_ids, _ = cube_object.find_bodies(".*")
+
+        # Sample a large force
+        external_wrench_b = torch.zeros(cube_object.root_view.count, len(body_ids), 6, device=self.sim.device)
+        external_wrench_b[0, 0, 2] = 9.81 * cube_object.root_view.get_masses(indices=[0])
+
+        # Now we are ready!
+        for _ in range(5):
+            # reset root state
+            root_state = cube_object.data.default_root_state.clone()
+            root_state[0, :2] = torch.tensor([0.0, -1.0], device=self.sim.device)
+            root_state[1, :2] = torch.tensor([0.0, 1.0], device=self.sim.device)
+            cube_object.write_root_state_to_sim(root_state)
+            # reset object
+            cube_object.reset()
+            # apply force
+            cube_object.set_external_force_and_torque(
+                external_wrench_b[..., :3], external_wrench_b[..., 3:], body_ids=body_ids
+            )
+            # perform simulation
+            for _ in range(50):
+                # apply action to the object
+                cube_object.write_data_to_sim()
+                # perform step
+                self.sim.step()
+                # update buffers
+                cube_object.update(self.dt)
+            # check condition that the objects have fallen down
+            self.assertLess(abs(cube_object.data.root_pos_w[0, 2].item()), 1e-3)
+            self.assertLess(cube_object.data.root_pos_w[1, 2].item(), -1.0)
+
+
+if __name__ == "__main__":
+    try:
+        unittest.main()
+    except Exception as err:
+        carb.log_error(err)
+        carb.log_error(traceback.format_exc())
+        raise
+    finally:
+        # close sim app
+        simulation_app.close()