ContactForceTorqueSensor update

Updated `ContactForceTorqueSensor` to work with the new collision detection. Does a lot of this by caching the collision detection data inside `TimeSteppingRigidBodyManipulator`. The test case passes, though, when I tried to add a second object to the test (via `addRobotFromURDF`) there were error messages on compilation. This seemed like a more general issue related to sensors, however.