3. Tutorial: Computing the inverse dynamics

This tutorial shows a short example how to compute the inverse dynamics using this package. In particular this means to compute the joint torques for a given motion. Since the joint torques are usually the system inputs, this is often referred to inverse dynamics. This computation allows us therefore to estimate the required joint torques to be applied for a desired motion using the model of the motion dynamics.

As a first step, we import the dynamics module which provides the functions of the model. We use a small helper to allow user input and output. You can find this module in the examples directory. The imports and the determination of the inputs then can look as follows:

import numpy as np
from lwrmodels.lwr4_q2q4 import dynamics
from helper import read_float_array, print_matrix, print_vector

# get the number of joints described by the model
n = dynamics.numJointsModel

# Read user inputs
print("Enter {} joint positions (space separated): ".format(n), end="")
q = np.array(read_float_array(n))
print("Enter {} joint velocities (space separated): ".format(n), end="")
dq = np.array(read_float_array(n))
print("Enter {} joint accelerations (space separated): ".format(n), end="")
ddq = np.array(read_float_array(n))

We can then compute the vectors and matrices of the model to determine the required actuation torque tau:

# Compute Inertia Matrix B
B = dynamics.get_inertia_matrix(q)

# Compute the Coriolis-/Centrifugal Matrix C
C = dynamics.get_coriolis_matrix(q, dq)

# Compute the gravity torque vector tau_g
tau_g = dynamics.get_gravity_torque_vector(q)

# Compute the friction torque vector tau_f
tau_f = dynamics.get_friction_torque_vector(dq)

# We can now compute the required actuation torques for this motion
tau = B.dot(ddq) + C.dot(dq) + tau_g + tau_f

In this example, we use the helper module to pretty-print the resulting matrices and vectors:

# Print the results
print("\n\nInput set to:")
print("    q = {}^T rad".format(str(q)))
print("   dq = {}^T rad/s".format(str(dq)))
print("  ddq = {}^T rad/s^2".format(str(ddq)))

print("\n\n\nOutput:")
print_matrix(B, "     B(q) = ")
print_matrix(C, "  C(q,dq) = ")
print_vector(tau_g, "     g(q) = ")
print_vector(tau_f, "tau_f(dq) = ")

print("Computed joint actuation torques:");
print_vector(tau, "tau = ")

3.1. Summary

You have seen how to access the function of this module to compute the inverse dynamics. You can try to extend this example to use the other models, lwr4_q1q2q4 and lwr4. In the next tutorial, we will compute the direct dynamics.

Table Of Contents

Previous topic

2. Reference

Next topic

4. Tutorial: Computing the direct dynamics

This Page