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.
careful.  the robot can have other state constraints, too (like loop joints, etc). 

also, there is already a mechanism for doing this using `resolveConstraints`, which takes a random vector in and solves a feasibility problem.  we want to make sure it's very clear how these two functions inter-relate.  For instance, the `getInitialState` method calls `resolveConstraints` on a random vector, but perhaps should call this first, then `resolveConstraints`