Inverse Kinematics

Done in MatLab though