add function to find shortest angle dist while respecting joint limits