Clean up the Matlab Implementation

[MatthewPeterKelly]

The SNS-IK library has a Matlab implementation. At this point, the Matlab code is intended for debugging and for doing basic algorithm testing before implementing the production version in C++. There are several TODO markers in the code, especially related to input validation and clean return values for edge cases. It would be great if anyone wanted to read through and make this Matlab code into a more production-ready implementation.

In addition to small changes, this ticket could also include refactoring to reduce the overall code duplication.

[robodreamer]

Sounds good. I can take on this after we finish the first iteration of getting all the core solvers pushed. We will probably need a document as well describing the algorithms in detail.