Various utilities

* new utilities related to quaternion floating base parameterization / difference between qdot and v
* gradient output for various utility functions
* gradient utilities
* snuck in a code formatting fix for findKinematicPath

An all-Matlab pull request adding various utility functions that shouldn't change any of the current functionality. The only exception is a matGradMultMat fix when the input is an empty matrix.
The majority of the methods used angularvel, so I went with that. I can change them all over to angularVel if this is a strong preference.