Avoiding singularity in testFloatingBaseDynamics.
Merge pull request #451 from tkoolen/issue-311

Avoid singularity in testFloatingBaseDynamics