Inverse dynamics#

The tensor API provides multiple functionalities to help users integrate inverse dynamics to their workflow. In the following, several typical use cases will be described.

Application of a joint-space trajectory#

A common use case is to estimate the forces/torques required to apply to a robot in order to follow a given joint-space trajectory. In other words, a method to convert the root link and joint accelerations into forces/torques applied to the root link and joints given the root link and joint positions and velocities.

This can be performed by using the equation of motion:

\[\tau = M(q) \ddot{q} + C(q, \dot{q}) * \dot{q} + G(q),\]

where \(q\) is the root link and joint positions, \({\tau}\) the forces/torques applied to the root link and joints, \(M(q)\) the mass matrix, \(C(q, \dot{q}) * \dot{q}\) the Coriolis and centrifugal forces, and \(G(q)\) the gravity forces.

The different terms of the equation of motion can be obtained using the following functions:

Using these properties, one can convert the desired root link and joint accelerations \(\ddot{q}\) to the required forces/torques \({\tau}\).

Note

The inverse dynamics calculation does not include the effects of damping, joint friction, and contact.

A usage example is provided below based on a simple gripper with a floating base. The inverse dynamics calculation is performed in the _apply_inverse_dynamics function. The original file is located at omni/extensions/ux/source/omni.physx.demos/python/scenes/InverseDynamicsTensorAPIDemo.py.

  1# SPDX-FileCopyrightText: Copyright (c) 2024-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
  2# SPDX-License-Identifier: BSD-3-Clause
  3#
  4
  5import carb
  6import numpy as np
  7import math
  8
  9from pxr import Gf, UsdGeom, PhysxSchema, UsdPhysics
 10import omni.physxdemos as demo
 11from omni.physx.scripts import physicsUtils
 12
 13from omni.physxdemos.utils import gripper_helpers
 14from omni.physxdemos.utils import numpy_utils
 15
 16class GripperInverseDynamicTensorAPIDemo(gripper_helpers.GripperDemoBase):
 17    title = "Gripper inverse dynamics"
 18    category = demo.Categories.COMPLEX_SHOWCASES
 19    short_description = "Gripper picking and moving a box using inverse dynamics functions"
 20    description = (
 21        "Demo showing how to use inverse dynamics functions to apply a given trajectory to an articulation."
 22    )
 23
 24    def __init__(self):
 25        super().__init__(enable_fabric=False)
 26
 27        # gripper param
 28        self._state = "Opening gripper"
 29        self._max_root_velocity = 0.1
 30        self._max_finger_velocity = 0.1
 31        self._gripper_open_pose = -0.35
 32        self._gripper_close_pose = 0.0
 33        self._gripper_top_position = 0.5
 34        self._gripper_bottom_position = 0.35
 35        self._gripper_initial_lateral_position = 0.0
 36        self._gripper_final_lateral_position = 0.3
 37        self._gripper_depth_position = 0.2
 38        self._prevPosGripper = 10.0 # arbitrary large value
 39
 40    def on_startup(self):
 41        pass
 42
 43    def create(self, stage):
 44        # setup the gripper
 45        super().create(stage=stage)
 46
 47        # add a box
 48        boxSize = 0.1
 49        boxColor = demo.get_primary_color()
 50        boxPosition = Gf.Vec3f(0.2, 0.05, 0.0)
 51        defaultPrimPath = stage.GetDefaultPrim().GetPath()
 52        boxActorPath = defaultPrimPath.AppendChild("boxActor")
 53        cubeGeom = UsdGeom.Cube.Define(stage, boxActorPath)
 54        cubePrim = stage.GetPrimAtPath(boxActorPath)
 55        cubeGeom.CreateSizeAttr(boxSize)
 56        half_extent = boxSize / 2
 57        cubeGeom.CreateExtentAttr([(-half_extent, -half_extent, -half_extent), (half_extent, half_extent, half_extent)])
 58        translateOp = cubeGeom.AddTranslateOp() 
 59        translateOp.Set(boxPosition)
 60        cubeGeom.AddScaleOp().Set(Gf.Vec3f(1.0, 1.0, 1.0)) 
 61        cubeGeom.CreateDisplayColorAttr().Set([boxColor])
 62        UsdPhysics.CollisionAPI.Apply(cubePrim)
 63        UsdPhysics.RigidBodyAPI.Apply(stage.GetPrimAtPath(boxActorPath))
 64        massAPI = UsdPhysics.MassAPI.Apply(stage.GetPrimAtPath(boxActorPath))
 65        massAPI.CreateMassAttr(0.005)
 66
 67    def on_tensor_start(self, tensorApi):
 68        sim = tensorApi.create_simulation_view("numpy")
 69        self._boxActor = sim.create_rigid_body_view("/World/boxActor")
 70        self._gripper = sim.create_articulation_view("/World/gripper/root")
 71        self._gripper_indices = np.arange(1, dtype=np.int32)
 72
 73        # prevent garbage collector from deleting the sim 
 74        self._sim = sim
 75
 76        return sim
 77
 78    def on_shutdown(self):
 79        self._boxActor = None
 80        self._gripper = None
 81        self._sim = None
 82        super().on_shutdown()
 83
 84    def on_physics_step(self, dt):
 85        posGripper = self._gripper.get_dof_positions()
 86        posRootGripper = self._gripper.get_root_transforms()
 87
 88        if self._state == "Opening gripper":
 89            # calculate desired acceleration
 90            expRootPose = np.array([self._gripper_depth_position, self._gripper_top_position, self._gripper_initial_lateral_position])
 91            expectedJointAccel, expectedRootAccel = self._calculate_desired_acceleration(dt, self._gripper_open_pose, expRootPose)
 92
 93            # apply joint force and root force required to achieve desired acceleration
 94            self._apply_inverse_dynamics(expectedJointAccel, expectedRootAccel)
 95
 96            # check if this stage is finished
 97            if np.abs(self._gripper_open_pose - posGripper[0][0]) < 0.001:
 98                self._state = "Moving down"
 99
100        elif self._state == "Moving down":
101            # calculate desired acceleration
102            expRootPose = np.array([self._gripper_depth_position, self._gripper_bottom_position, self._gripper_initial_lateral_position])
103            expectedJointAccel, expectedRootAccel = self._calculate_desired_acceleration(dt, self._gripper_open_pose, expRootPose)
104
105            # apply joint force and root force required to achieve desired acceleration
106            self._apply_inverse_dynamics(expectedJointAccel, expectedRootAccel)
107
108            # check if this stage is finished
109            if np.abs(self._gripper_bottom_position - posRootGripper[0][1]) < 0.001:
110                self._state = "Grasping"
111
112        if self._state == "Grasping":
113            # calculate desired acceleration
114            expRootPose = np.array([self._gripper_depth_position, self._gripper_bottom_position, self._gripper_initial_lateral_position])
115            expectedJointAccel, expectedRootAccel = self._calculate_desired_acceleration(dt, self._gripper_close_pose, expRootPose)
116
117            # apply joint force and root force required to achieve desired acceleration
118            self._apply_inverse_dynamics(expectedJointAccel, expectedRootAccel)
119
120            # check if this stage is finished (when the gripper is not moving anymore)
121            if np.abs(self._prevPosGripper - posGripper[0][0]) < 0.001:
122                self._state = "Moving up"
123
124            self._prevPosGripper = posGripper[0][0]
125
126        elif self._state == "Moving up":
127            # calculate desired acceleration
128            expRootPose = np.array([self._gripper_depth_position, self._gripper_top_position, self._gripper_initial_lateral_position])
129            expectedJointAccel, expectedRootAccel = self._calculate_desired_acceleration(dt, self._gripper_close_pose, expRootPose)
130
131            # apply joint force and root force required to achieve desired acceleration
132            self._apply_inverse_dynamics(expectedJointAccel, expectedRootAccel)
133
134            # check if this stage is finished
135            if np.abs(self._gripper_top_position - posRootGripper[0][1]) < 0.001:
136                self._state = "Moving laterally"
137
138        elif self._state == "Moving laterally":
139            # calculate desired acceleration
140            expRootPose = np.array([self._gripper_depth_position, self._gripper_top_position, self._gripper_final_lateral_position])
141            expectedJointAccel, expectedRootAccel = self._calculate_desired_acceleration(dt, self._gripper_close_pose, expRootPose)
142
143            # apply joint force and root force required to achieve desired acceleration
144            self._apply_inverse_dynamics(expectedJointAccel, expectedRootAccel)
145
146            # check if this stage is finished
147            if np.abs(self._gripper_final_lateral_position - posRootGripper[0][2]) < 0.001:
148                self._state = "Dropping"
149
150        elif self._state == "Dropping":
151            # calculate desired acceleration
152            expRootPose = np.array([self._gripper_depth_position, self._gripper_top_position, self._gripper_final_lateral_position])
153            expectedJointAccel, expectedRootAccel = self._calculate_desired_acceleration(dt, self._gripper_open_pose, expRootPose)
154
155            # apply joint force and root force required to achieve desired acceleration
156            self._apply_inverse_dynamics(expectedJointAccel, expectedRootAccel)
157
158        super().on_physics_step(dt)
159
160    def _calculate_desired_acceleration(self, dt, expFingerPos, expRootPose):
161        # get position and velocity of the gripper
162        posGripper = self._gripper.get_dof_positions()
163        velGripper = self._gripper.get_dof_velocities()
164        posRootGripper = self._gripper.get_root_transforms()
165        velRootGripper = self._gripper.get_root_velocities()
166
167        # calculate velocity to achieve desired pose
168        predFingerVelocity = (expFingerPos - posGripper[0][0]) / dt
169        predRootVelocity = (expRootPose[0:3] - posRootGripper[0][0:3]) / dt
170
171        # clamp velocity to maximum allowed velocity
172        predFingerVelocity = np.sign(predFingerVelocity) * np.minimum(np.abs(predFingerVelocity), self._max_finger_velocity)
173        predRootVelocity = np.sign(predRootVelocity) * np.minimum(np.abs(predRootVelocity), self._max_root_velocity)
174
175        # correct root angular position
176        rootOrientation = np.array([0.0, 0.0, -0.7071068, 0.7071068])
177        qErr = numpy_utils.quat_mul(rootOrientation, numpy_utils.quat_conjugate(posRootGripper[0][3:]))
178        predRootRotXVelocity = math.atan2(2 * (qErr[3] * qErr[0] + qErr[1] * qErr[2]), 1 - 2 * (qErr[0]**2 + qErr[1]**2)) / dt
179        predRootRotYVelocity = math.asin(2 * (qErr[3] * qErr[1] - qErr[2]* qErr[0])) / dt
180        predRootRotZVelocity = math.atan2(2 * (qErr[3] * qErr[2] + qErr[0] * qErr[1]), 1 - 2 * (qErr[1]**2 + qErr[2]**2)) / dt
181
182        # calculate desired acceleration
183        predFingerAccel = (predFingerVelocity - velGripper[0][0]) / dt
184        predRootAccel = (predRootVelocity - velRootGripper[0][0:3]) / dt
185        predRootRotXAccel = (predRootRotXVelocity - velRootGripper[0][3]) / dt
186        predRootRotYAccel = (predRootRotYVelocity - velRootGripper[0][4]) / dt
187        predRootRotZAccel = (predRootRotZVelocity - velRootGripper[0][5]) / dt
188
189        return [predFingerAccel, -predFingerAccel], [predRootAccel[0], predRootAccel[1], predRootAccel[2], predRootRotXAccel, predRootRotYAccel, predRootRotZAccel]
190
191    def _apply_inverse_dynamics(self, expectedJointAccel, expectedRootAccel):
192        rootDofs = 6 # Degree of freedoms of the root: 0 for fixed-base articulation, 6 for floaitng-base articulations
193        coriolisCompensation = self._gripper.get_coriolis_and_centrifugal_compensation_forces()
194        gravityCompensation = self._gripper.get_gravity_compensation_forces()
195        massMatrixReshape = self._gripper.get_generalized_mass_matrices().reshape(self._gripper.max_dofs + rootDofs,self._gripper.max_dofs + rootDofs)
196
197        # calculate force to achieve given acceleration
198        requiredJointForce = np.matmul(massMatrixReshape[rootDofs:, rootDofs:], expectedJointAccel)
199        requiredJointForce += np.matmul(massMatrixReshape[rootDofs:, :rootDofs], expectedRootAccel)
200        requiredRootForce = np.matmul(massMatrixReshape[:rootDofs, rootDofs:], expectedJointAccel)
201        requiredRootForce += np.matmul(massMatrixReshape[:rootDofs, :rootDofs], expectedRootAccel)
202
203        # add gravity and Coriolis compensation force
204        requiredJointForce += coriolisCompensation[0][rootDofs:] + gravityCompensation[0][rootDofs:]
205        requiredRootForce += coriolisCompensation[0][:rootDofs] + gravityCompensation[0][:rootDofs]
206
207        # apply required joint force
208        self._gripper.set_dof_actuation_forces(requiredJointForce, self._gripper_indices)
209
210        # apply required root force
211        forces = np.zeros(self._gripper.max_links * 3)
212        torques = np.zeros(self._gripper.max_links * 3)
213        forces[0:3] = requiredRootForce[0:3]
214        torques[0:3] = requiredRootForce[3:6]
215        self._gripper.apply_forces_and_torques_at_position(forces, torques, None, self._gripper_indices, True)

To run the demo:

  1. Enables the demos from the menu Window > Physics > Demo Scenes.

  2. In the Physics Demo Scenes tab, the demo is under the Complex Showcases category and is named Gripper inverse dynamics.

Centroidal momentum#

The tensor API provides utilities to predict the evolution of the centroidal momentum \(h_G\) using the velocity \(\dot{q}\) and acceleration \(\ddot{q}\) of the combined root link and joint degrees-of-freedom of a floating-base articulation.

This can be done using the centroidal momentum matrix \(A_G\) and the corresponding bias force \(\dot{A}_G \dot{q}\):

\[h_G = A_G \dot{q}\]

and

\[\dot{h}_G = A_G \ddot{q} + \dot{A}_G \dot{q}.\]

The centroidal momentum matrix and the corresponding bias force of an articulation can be obtained using the function get_articulation_centroidal_momentum.

Note

The centroidal momentum matrix does not include the effects of damping, joint friction, and contact. Moreover, the functionality is only implemented for floating-base articulations.

A minimal usage example is provided below:

import asyncio
import numpy as np
import omni.physics.tensors as tensors

async def centroidal_momentum_use_case():
    # Wait for a frame to ensure the physics simulation is initialized
    await omni.kit.app.get_app_interface().next_update_async()

    # Create the simulation view
    sim_view = tensors.create_simulation_view("warp")

    # Create the articulation view
    articulation_view = sim_view.create_articulation_view("/World/envs/env_0/Humanoid")

    # Get the centroidal momentum matrix and bias force for all articulations in the view
    temp_buffer = articulation_view.get_articulation_centroidal_momentum()
    temp_buffer_np = temp_buffer.numpy().reshape(articulation_view.count, 6, articulation_view.max_dofs + 7)

    # Extract the centroidal momentum matrix and the bias force from the results
    centroidal_momentum_matrices_np = temp_buffer[:, :, :-1]
    bias_forces_np = temp_buffer[:, :, -1:]

    # Set arbitrary joint and root velocities for the sake of the example
    root_velocity = np.zeros([articulation_view.count, 6])
    joint_velocity = np.zeros([articulation_view.count, articulation_view.max_dofs])
    for i in range(articulation_view.count):
        root_velocity[i, :] = np.array([0.0, 0.1 * i, -0.1, 0.0, 0.4 * i, 0.0])
        for j in range(articulation_view.max_dofs):
            joint_velocity[i, j] = 0.1 * i - 0.5 * j

    # Calculate expected centroidal momentum
    expected_centroidal_momentum = np.zeros([articulation_view.count, 6])
    for i in range(articulation_view.count):
        expected_centroidal_momentum[i, :] = np.dot(centroidal_momentum_matrices_np[i, :, 6:], joint_velocity[i, :])
        expected_centroidal_momentum[i, :] += np.dot(centroidal_momentum_matrices_np[i, :, :6], root_velocity[i, :])

# Run the simulation
timeline = omni.timeline.get_timeline_interface()
timeline.play()
asyncio.ensure_future(centroidal_momentum_use_case())

The above example may be used in the script editor as follows:

  1. First, open the asset omni/data/usd/tests/Physics/HumanoidOneEnv.usd.

  2. Open the script editor from the menu Window > Script Editor.

  3. Paste the following snippet in the script editor and run it