thorough testing, cleanup, and re-working of lookat_ik, ready for testing ros interface on robot