Fix #95.

Uninitialized `kinsol` as argument to `collisionDetect` was causing a crash if
the test failed. Replaced `kinsol` with `q0`.
Merge pull request #96 from avalenzu/collision-minor-changes

Fix #95.