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.

The idea is that in the future, a RigidBody will have a `std::unique_ptr<DrakeJoint>`, which will be used for computation in the kinematics/dynamics algorithms.