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.

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

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