This tutorial shows a short example how to compute the direct dynamics using this package. In particular this means to compute the joint accelerations for given joint positions, joint velocities and joint torques.
As in the previous tutorial, 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. Instead of the joint accelerations ddq, we read the actuation torques tau from the user. 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 actuation torques (space separated):".format(n),
end="")
tau = np.array(read_float_array(n))
We can then compute the vectors and matrices of the model to estimate the resulting joint accelerations ddq. Note that this requires the inertia matrix B to be inverted.
# Compute Inertia Matrix B
B = dynamics.get_inertia_matrix(q)
# Compute the inverse of the Inertia Matrix
Binv = np.linalg.inv(B)
# 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 resulting joint accelerations ddq
ddq = Binv.dot( tau - C.dot(dq) - tau_g - tau_f )
Again, let us write the results to the console:
# Print the results
print("\n\nInput set to:")
print(" q = {}^T rad".format(str(q)))
print(" dq = {}^T rad/s".format(str(dq)))
print(" tau = {}^T Nm".format(str(tau)))
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_matrix(Binv, " B(q)^-1 = ")
print("Computed joint accelerations:");
print_vector(ddq, "ddq = ")
You have seen how to access the function of this module to compute the direct dynamics using numpy to invert the inertia matrix. You can try to extend this example to use the other models, lwr4_q1q2q4 and lwr4.