Cinemática Inversa 5 gdl

Hola a todos. Estoy construyendo un robot de 5 gdl y tengo que usar la cinematica invversa para calcular trayectorias y todo eso. Se trata de un robot con las 5 articulaciones de rotacion . los valores de Denavit Hatenberg son

teta = [q1 q2 q3 q4+sym('pi') q5 ]; d = [d1 0 0 0 d5+d4 ]; a = [0 d2 d3 0 0 ]; alfa = [sym('pi')/2 0 0 sym('pi')/2 0 ];

La directa me funciona perfectamente pero no consigo hacer la inversa. Se supone que el quinto grado no afectaria porque no afecta a la posicion. estoy utilizando este codigo: (En codigo de Matlab ya que lo estoy probando ahi)

Hola Mac26 , disculpa estoy tratando de hacer cinematica directa con un brazo robotico, talvez me podrias facilitar el codigo o un ejemplo de la misma ya que no tengo una nocion clara de como empezar a programar con los parametros de DH.