Fix corner cases for pitch angles, avoid divide by zero (or very small)

Note this in the symbolic code that generates these solutions

Author: petercorke

symbolic/rpygen.mlx

@@ -28,6 +28,7 @@
 % - Toolbox rel 8-9 has XYZ angle sequence as default.
 % - 'arm', 'vehicle', 'camera' are synonyms for 'xyz', 'zyx' and 'yxz'
 %   respectively.
+% - these solutions are generated by symbolic/rpygen.mlx
 %
 % See also  rpy2tr, tr2eul.
 
@@ -87,7 +88,17 @@
                 rpy(1) = -atan2(R(1,2), R(1,1));
                 rpy(3) = -atan2(R(2,3), R(3,3));
 
-                rpy(2) = atan(R(1,3)*cos(rpy(1))/R(1,1));
+                [~,k] = max(abs( [R(1,1) R(1,2) R(2,3) R(3,3)] ));
+                switch k
+                    case 1
+                        rpy(2) =  atan(R(1,3)*cos(rpy(1))/R(1,1));
+                    case 2
+                        rpy(2) = -atan(R(1,3)*sin(rpy(1))/R(1,2));
+                    case 3
+                        rpy(2) = -atan(R(1,3)*sin(rpy(3))/R(2,3));
+                    case 4
+                        rpy(2) =  atan(R(1,3)*cos(rpy(3))/R(3,3));
+                end
             end
 
         case {'zyx', 'vehicle'}
@@ -106,8 +117,18 @@
             else
                 rpy(1) = atan2(R(3,2), R(3,3));  % R
                 rpy(3) = atan2(R(2,1), R(1,1));  % Y
-                
-                rpy(2) = -atan(R(3,1)*cos(rpy(1))/R(3,3));
+                     
+                [~,k] = max(abs( [R(1,1) R(2,1) R(3,2) R(3,3)] ));
+                switch k
+                    case 1
+                        rpy(2) = -atan(R(3,1)*cos(rpy(3))/R(1,1));
+                    case 2
+                        rpy(2) = -atan(R(3,1)*sin(rpy(3))/R(2,1));
+                    case 3
+                        rpy(2) = -atan(R(3,1)*sin(rpy(1))/R(3,2));
+                    case 4
+                        rpy(2) = -atan(R(3,1)*cos(rpy(1))/R(3,3));
+                end
             end
 
         case {'yxz', 'camera'}
@@ -127,7 +148,17 @@
                 rpy(1) = atan2(R(2,1), R(2,2));
                 rpy(3) = atan2(R(1,3), R(3,3));
 
-                rpy(2) = -atan(cos(rpy(1))*R(2,3)/R(2,2));
+                [~,k] = max(abs( [R(2,1) R(2,2) R(1,3) R(3,3)] ));
+                switch k
+                    case 1
+                        rpy(2) = -atan(R(2,3)*sin(rpy(1))/R(2,1));
+                    case 2
+                        rpy(2) = -atan(R(2,3)*cos(rpy(1))/R(2,2));
+                    case 3
+                        rpy(2) = -atan(R(2,3)*sin(rpy(3))/R(1,3));
+                    case 4
+                        rpy(2) = -atan(R(2,3)*cos(rpy(3))/R(3,3));
+                end
             end
 
     end