Avoid singularity in testFloatingBaseDynamics

Should fix #311.