Updated pseudoinverse
Merge branch 'pleasework' of https://github.com/nashj/Mobile-Manipulator into pleasework