Problema de singularidad en solucionador cinemático inverso

Estoy luchando con este problema por días. Realmente espero que alguien pueda darme una pista de cuál es el problema. El robot consta de 5 ejes. El primer eje gira alrededor del eje z y otros 4 ejes giran alrededor del eje y. Y el solucionador básicamente funciona. Esto es lo que he hecho hasta...