New doKinematics, geometricJacobian

* added `use_new_kinsol` boolean to `RigidBodyManipulator`
* copied new `doKinematics` and `geometricJacobian` Matlab code over from floatingBase branch, gets called if `use_new_kinsol`
* ported `doKinematics` and `geometricJacobian` code to C++, also gets called if `use_new_kinsol`

The idea of this pull request is that everything should still run the old code (i.e. all tests should pass), while making it possible to switch to the new implementation by setting a single boolean. `testGeometricJacobian` is the first test to pass regardless of whether `use_new_kinsol` is true. `geometricJacobian` mostly serves as a proof that at least parts of the new `doKinematics` code work. Using this as a starting point, I should now be able to do a series of smaller pull requests, pulling in functions from floatingBase and porting them to C++ one by one, while retaining old functionality exactly.