MemoryError when calling calcDiff on CostModel
I am developing and debugging cost models right now. I noticed that the memory for an object retrieved from Pinocchio is broken (Jcom, the number of columns equal "overflow" so it runs out of memory).
I may have a mistake in how I am compiling the script - can you please have a look to check? This is the script I am using to reproduce on the latest devel with the latest devel of eigenpy and pinocchio:
from __future__ import print_function
import numpy as np
import eigenpy
eigenpy.switchToNumpyMatrix()
import crocoddyl
import pinocchio
# print(pinocchio.__file__)
import example_robot_data
robot = example_robot_data.loadANYmal()
robot_model = robot.model
pinocchio_data = robot_model.createData()
state = crocoddyl.StateMultibody(robot_model)
actuation = crocoddyl.ActuationModelFloatingBase(state)
com_position = crocoddyl.CostModelCoMPosition(state, np.matrix([0.,0.,0.5]).T)
com_position_data = com_position.createData(pinocchio_data)
q0 = robot_model.referenceConfigurations['half_sitting']
v0 = np.asmatrix([0.] * robot_model.nv).T
x0 = np.concatenate([q0, v0])
pinocchio.forwardKinematics(robot_model, com_position_data.pinocchio, q0)
com = pinocchio.centerOfMass(robot_model, com_position_data.pinocchio, q0)
Jcom = pinocchio.jacobianCenterOfMass(robot_model, com_position_data.pinocchio, q0)
com_position.calc(com_position_data, x0)
print("calc done")
com_position.calcDiff(com_position_data, x0, np.asmatrix([0.] * robot_model.nv).T) # <-- MemoryError here
print("calcDiff done")
Inspecting the dimensions of pinocchio->Jcom
inside the cost model shows 3 x (super huge number that is overflowing unsigned int)
.