Now robot to world translation works with nice math