Drake geometry util additions

Added rotation conversion functions + test. Moved angleDiff to drakeGeometryUtil.
uniformlyRandomRPY ?