¿Cómo convierto los parámetros y ángulos de enlace (en cinemática) en matrices de transformación en lógica de programación?

Estoy haciendo investigación en robótica como estudiante universitario, y entiendo la matemática conceptual en su mayor parte; sin embargo, cuando se trata de implementar código para calcular la cinemática directa de mi robot, estoy atascado. Simplemente no entiendo cómo el libro o los sitios web que he encontrado lo explican.

Me gustaría calcular los ángulos XYZ dados los parámetros del enlace (parámetros de Denavit-Hartenberg), como los siguientes :

$$\begin{array}{ccc} \bf{i} & \bf{\alpha_i-1} & \bf{a_i-1} & \bf{d_i} & \bf{\theta_i}\\ \\ 1 & 0 & 0 & 0 & \theta_1\\ 2 & -90^{\circ} & 0 & 0 & \theta_2\\ 3 & 0 & a_2 & d_3 & \theta_3\\ 4 & -90^{\circ} & a_3 & d_4 & \theta_4\\ 5 & 90^{\circ} & 0 & 0 & \theta_5\\ 6 & -90^{\circ} & 0 & 0 & \theta_6\\ \end{array}$$

No entiendo cómo convertir esta tabla de valores en las matrices de transformación adecuadas necesarias para obtener , la posición cartesiana y la rotación del último enlace. A partir de ahí, espero poder averiguar el ángulo (s) XYZ al leer mi libro, pero agradecería cualquier ayuda.$^0T_N$

La sección Matriz DH de la página DH en wikipedia tiene los detalles.

Básicamente, desea utilizar la información en su tabla para crear un conjunto de matrices de transformación homogéneas. Lo hacemos porque las transformaciones homogéneas se pueden multiplicar para encontrar la relación entre cuadros separados por uno o más. Por ejemplo, representa la transformación del cuadro 1 al cuadro 0, mientras que representa la transformación del cuadro 2 al cuadro 1. Al multiplicarlos obtenemos la transformación del cuadro 2 al cuadro 0, es decir, .$^0T_1$$^1T_2$$^0T_2 = ^0T_1^1T_2$

Una manera fácil de crear cada una de las transformaciones es realizar una transformación homogénea o una matriz de rotación homogénea para cada columna de la tabla y multiplicarlas. Por ejemplo, la transformación de 1 a 0 (por ejemplo, ) es$^{i-1}T_i, i = 1$

$^0T_1 = Trans(d_1)*Rot(\theta_1)*Trans(a_2)*Rot(\alpha_2)$

dónde

$Trans(d_1) = \begin{bmatrix}1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & \bf{d_1 = 0} \\ 0 & 0 & 0 & 1 \end{bmatrix},$

$Rot(\theta_1) = \begin{bmatrix} \text{cos}(\bf{\theta_1}) & - \text{sin}(\bf{\theta_1}) & 0 & 0 \\ \text{sin}(\bf{\theta_1}) & \text{cos}(\bf{\theta_1}) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix},$

$Trans(a_2) = \begin{bmatrix} 1 & 0 & 0 & \bf{a_2 = 0} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix},$

$Rot(\alpha_2) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \text{cos}(\bf{\alpha_2 = 0}) & -\text{sin}(\bf{\alpha_2 = 0}) & 0 \\ 0 & \text{sin}(\bf{\alpha_2 = 0}) & \text{cos}(\bf{\alpha_2 = 0}) & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$ .

En este caso

$^0T1 = Rot(\theta_1)$ .

Una vez que tenga todas sus transformaciones, las multiplicará juntas, p. Ej.

$^0T_N = ^0T_1*^1T_2...^{N-1}T_N$ .

Finalmente, puede leer el vector de desplazamiento de la transformación homogénea (es decir, ). Del mismo modo, puede leer la matriz de rotación de para encontrar los ángulos XYZ.$^0T_N$$d = [^0T_{N,14}, ^0T_{N,24}, ^0T_{N,34}]^T$$^0T_N$