Problema de singularidade no solucionador cinemático inverso

Estou lutando com esse problema há dias. Eu realmente espero que alguém possa me dar uma dica de qual é o problema. O robô é composto por 5 eixos. O primeiro eixo gira em torno do eixo z e outros 4 eixos giram em torno do eixo y. E o solucionador basicamente funciona. Aqui está o que eu fiz até...