Restore center of mass in the presence of inertia matrix

[ad-daniel]

If the inertia matrix is provided, the center of mass must also be provided, even if it's at the default value. Otherwise warnings are shown.