cleaning up twist_controller + use augmented_solver, currently pinv with SVD and manipulability analysis