implement Inverse Kinematics in Robot #41
Description
Implement the Inverse Kinematics so the current scaled down version can move.
See the inverse kinematics branch for a working implementation of the IK required.
The end goal is for us to give the robot a position/action to move to/do and have the robot do it.