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.
I don't want to get hung up on this right now, so I'm going to move this method to systems/plants/test and adding a %NOTEST.