fix bugs in ManipulationCentroidalDynamicsFullKinematicsPlanner, it can run now