started forward kinematics implementation