pinocchio_crba_consistency.py

#!/usr/bin/python

import numpy as np
import pinocchio
import sys

model_file = sys.argv[1]
floating_base = False

for i in range(1, len (sys.argv[1:]) + 1):
    arg = sys.argv[i]
    if arg == "-f":
        floating_base = True

pin_model = pinocchio.buildModelFromUrdf (model_file, floating_base)
pin_data = pin_model.createData()
pin_q = np.zeros ([pin_model.nq, 1])
pin_qdot = np.ones ([pin_model.nv, 1])

print ("Model: " + model_file) 
print ("--")
print ("Stats      |  #q | #qdot ")
print ("{:>10s} | {: 3d} | {: 3d}".format ("Pinocchio", pin_model.nq, pin_model.nv))
print ("--")

M_crba = np.zeros([pin_model.nv, pin_model.nv])
M_rnea = np.zeros([pin_model.nv, pin_model.nv])
M_col = np.zeros([pin_model.nv, 1])
qddot0 = np.zeros([pin_model.nv, 1])
qddot_i = np.zeros([pin_model.nv, 1])
accel0 = np.zeros([pin_model.nv, 1])
accel_i = np.zeros([pin_model.nv, 1])

# ensure that we have a valid quaternion for the free-flyer joint
if floating_base:
    pin_q[6] = 1.

accel0 = pinocchio.rnea (pin_model, pin_data, pin_q, pin_qdot, qddot0)

for i in range (pin_model.nv):
    qddot_i[i] = 1.
    accel_i = pinocchio.rnea (pin_model, pin_data, pin_q, pin_qdot, qddot_i)
    M_rnea[:,i] = np.reshape (accel_i - accel0, (pin_model.nv,))
    qddot_i[i] = 0.

M_crba = pinocchio.crba (pin_model, pin_data, pin_q)

np.set_printoptions(precision=2)

print ("Error M[0:6,0:6]")
print ((M_crba - M_rnea)[0:6,0:6])
print ("Error M[0:6,6:12]")
print ((M_crba - M_rnea)[0:6,6:12])