IKSolver seems to be working! Done with Eigen::JacobiSVD