WIP: Floating contact dynamics and impact dynamics

This PR takes care of the bug in locomotion related to the sign of the contact forces. This PR also takes care of the #102 (closed)

Please take care that this PR changes the impact dynamics. This PR now implements Mdv=JtF as the impact dynamics.

This PR also removes the (-f) notation from floating_in_contact

The changes include changes to the unittest as well.