End Effector Kinematics Calculations

WIP, will rewrite things properly once calculations have been verified.