finished up the functions to calculate the end effectors position and orientation