Matriz de transformación en Python

Una matriz de transformación la podemos visualizar como como una función lleva los valores escalares de las articulaciones (ya sean ángulos o desplazamientos) a cuatro vectores. Los primeros tres corresponden a las componentes de los ejes $x_i$, $y_i$ y $z_i$ en los ejes $x_j$, $y_j$ y $z_j$ y el cuarto a las coordenadas del origen $O_i$ en el sitema $j$.

Nota: los prefijos $i$ y $j$ significa que son sistemas de coordenadas diferentes.

Si seguimos la metodología de Denavit Hartenberg entonces podemos generar nuestra matriz de transformación tomando como entrada los valores $[a_i$, ${\alpha }_i$, $d_i$, ${\theta }_i]$ del eslabón $i$ y podemos determinar los cuatro vectores que describimos anteriormente de forma independiente para facilitar la lectura del código y al final concatenarlos en una matriz.

Nota: es importante asegurarse que se retorne una matriz y no un array para que la multiplicación sea multiplicación de matrices y no elemento a elemento. Esto también permite usar otras fuciones de numpy que requieren matrices como argumentos de entrada.

Usare un Closure, que es como una función que devuelve otra función, como matriz de transformación. Si bien no la podremos visualizar con variables simbolicas, la podemos visualizar para cada conjunto de valores de las articulaciones que le suministremos.

La función queda así:

Nota: me hubiera gustado usar tipado estático pero no encontre un módulo de Python que me permitiera usarlo con variables de tipo matriz.

Ejemplo

Supondremos que los eslabones $2$ y $3$ del robot tienen una longitud entre ajes $z$ de $30$ $cm$ dandonos los suguientes parametros de Denavit Hartenberg

Con estas listas creamos las matrices de transformación, que para nosotros seran funciones que al recibir un angulo nos dara la orientación y posición de un eslabón en relación al eslabón anterior.

Por ejemplo, el primer eslabón no puede modificar desplazamiento en ningún eje, pero si su orientación en relación al sistema de referencia $0$. Por ejmeplo, para $\theta =0$, los ejes $x$ siguen alineados, el eje $y_1$ es paralelo al eje $z_0$ y el eje $z_1$ es paralelo y contrario a $y_0$.

Para obtener las coordenadas del marco coordemado $O_3$ multiplicamos las tres matrices, por ejemplo, si todos los ángulos son $0^{\circ }$ entonces el origen $O_3$ esta a $60cm$ sobre el eje x, y su orientación sería la misma que en el ejemplo anterior.

Nos enfocaremos en la posición, es decir, el vector $d$ de las matrices de transformación.