Added method to compute the state from the final end-effector positions. Currently, tries to compute multiple answers and is unfortunately incorrect.