def __init__(self, state, actuationModel, costModel):
crocoddyl.DifferentialActionModelAbstract.__init__(self, state, actuationModel.nu, costModel.nr)
self.actuation = actuationModel
self.costs = costModel
self.enable_force = True
self.armature = np.matrix(np.zeros(0))
# We cannot abstract data in Python bindings, let's create this internal data inside model
self.pinocchioData = pinocchio.Data(self.state.pinocchio)
self.multibodyData = crocoddyl.DataCollectorMultibody(self.pinocchioData)
self.actuationData = self.actuation.createData()
self.costsData = self.costs.createData(self.multibodyData)
self.Minv = None
def setUp(self):
self.robot_data = self.ROBOT_MODEL.createData()
self.x = self.ROBOT_STATE.rand()
self.u = pinocchio.utils.rand(self.ROBOT_MODEL.nv)
self.multibody_data = crocoddyl.DataCollectorMultibody(self.robot_data)
self.data = self.COST.createData(self.multibody_data)
self.data_der = self.COST_DER.createData(self.multibody_data)
nq, nv = self.ROBOT_MODEL.nq, self.ROBOT_MODEL.nv
pinocchio.forwardKinematics(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:])
pinocchio.computeForwardKinematicsDerivatives(self.ROBOT_MODEL, self.robot_data, self.x[:nq], self.x[nq:],
pinocchio.utils.zero(nv))
pinocchio.computeJointJacobians(self.ROBOT_MODEL, self.robot_data, self.x[:nq])
pinocchio.updateFramePlacements(self.ROBOT_MODEL, self.robot_data)
pinocchio.jacobianCenterOfMass(self.ROBOT_MODEL, self.robot_data, self.x[:nq], False)