Preliminary working version. To be tuned and debugged for proper kinematic.
Once this is working, teleop_ik will be split into a twist command for the arm
and a teleop node.