Merge pull request #6 from mposa/collision-dynamics-integration

Adds capsule tests and speeds up contactConstraints
Merge pull request #7 from mposa/collision-dynamics-integration

Collision dynamics integration
Fix off-by-one error on body indices.

This was causing the last link in the chain to be ignored in
collisionDetection.
Fix output method for TSRBM with multiple robots.

Use the transformation we already have from TSRBM state frame to RBM state
frame to extract the manipulator state. We'll need to add transforms from TSRBM
state to the sensor states as well.
Fixed testContactGradients.
Fixes to test programs. Fixed ground contact case in contactConstraints.