add a test for ManipulationCentroidalDynamicsFullKinematicsPlanner