Implemented joint position calculation