Cinemática inversa de un manipulador 3gdl (método numérico)

Simulación de un Manipulador 3 GDL (RRR)
Documentación
Objetivo: Simulación en Mathematica de la Cinemática Directa e Inversa de un Brazo Manipulador de 3 grados de libertad de configuración RRR.Métodos Numéricos Utilizados:
Para la cinemática inversa del manipulador se eligió utilizar el método jacobiano inverso.
En este método se define la matriz jacobiana para posteriormente sacarla inversa de esta matriz. La inversa de la matriz se calculó con el método numérico de Gauss-Jordan para matrices inversas.
Posteriormente teniendo la matriz jacobiana invertida se utiliza el métodode Newton para calcular la cinemática inversa del manipulador.
Diagrama

Cinemática Directa
Matrices Homogéneas
Para la definición de las ecuaciones que definen al sistema se utilizó el Método deMatrices de Transformación Homogéneas.
* Matriz de Rotación en Z

* Matriz de Rotación en X

* Matriz de Rotación en Y

* Matriz de Traslación

Para nuestro brazo tenemos
Pf=Rz,θ1 Tz, L0 Ry, θ2 Tx, L1 Ry, θ3 T x, L2 Po
Al resolver el producto punto entre las matrices y tomando Po={0,0,0,1} tenemos nuestras ecuaciones para el sistema:
X =L1Cos[θ1]Cos[θ2]+L2(Cos[θ1]Cos[θ2]Cos[θ3]-Cos[θ1]Sin[θ2]Sin[θ3])
Y = L1Cos[θ2]Sin[θ1]+L2(Cos[θ2]Cos[θ3]Sin[θ1]-Sin[θ1]Sin[θ2]Sin[θ3])
Z = L0+L1Sin[θ2]+L2(Cos[θ3]Sin[θ2]+Cos[θ2]Sin[θ3])
Cinemática Inversa
Matriz JacobianaPara la cinematica inversa del problema elegimos utilizar la Matriz Jacobiana como una aproximación lineal de nuestro sistema de ecuaciones no lineales de nuestro manipulador.

La matriz jacobiana esla matriz hecha por las derivadas parciales de las funciones que describen al sistema respecto a sus grados de libertad. Permite conocer las velocidades del extremo del robot a partir de lasvelocidades de cada articulación.
e=Jθ θ
Matriz Jacobiana Inversa y Método de Newton
La razón por la cual se calcula el jacobiano es por la aproximación que se puede hacer utilizando el método de...