Updated IK solution
Everything compiles in Hydro
Cleaned up old Gazebo code