Fixed testContactGradients.
Fixes to test programs. Fixed ground contact case in contactConstraints.
Merge pull request #10 from mposa/collision-dynamics-integration

Collision dynamics integration