Fixed getClosestSolution in baxter_ikfast_right_arm_plugin