Factorize the checkMatch* code.
Add safe function to CoMJacobian, CentroidalMomentumMatrix and Jacobian.