forward-dynamics-derivatives.py

Go to the documentation of this file.

import numpy as np
import pinocchio as pin
 
 
 
# Create model and data
 
model = pin.buildSampleModelHumanoidRandom()
data = model.createData()
 
# Set bounds (by default they are undefinded for a the Simple Humanoid model)
 
model.lowerPositionLimit = -np.ones((model.nq, 1))
model.upperPositionLimit = np.ones((model.nq, 1))
 
q = pin.randomConfiguration(model)  # joint configuration
v = np.random.rand(model.nv, 1)  # joint velocity
tau = np.random.rand(model.nv, 1)  # joint acceleration
 
# Evaluate the derivatives
 
pin.computeABADerivatives(model, data, q, v, tau)
 
# Retrieve the derivatives in data
 
ddq_dq = data.ddq_dq  # Derivatives of the FD w.r.t. the joint config vector
ddq_dv = data.ddq_dv  # Derivatives of the FD w.r.t. the joint velocity vector
ddq_dtau = data.Minv  # Derivatives of the FD w.r.t. the joint acceleration vector