Drake joints

* Added DrakeJoint and subclasses in C++. These contain all the joint related methods in C++. Added test that compares Matlab and C++ implementations.
* Ported necessary angular velocity <-> rpydot and angular velocity <-> quatdot functions from Matlab + comparison.
* Added gradients to some rotation conversion functions.