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.