Forward kinematics, inverse need debugging. Code is done