Come posso convertire i parametri e gli angoli dei collegamenti (in cinematica) in matrici di trasformazione nella logica di programmazione?

Sto facendo ricerche di robotica come studente universitario e capisco la matematica concettuale per la maggior parte; tuttavia, quando si tratta di implementare effettivamente il codice per calcolare la cinematica diretta per il mio robot, sono bloccato. Semplicemente non capisco come lo spiegano il libro o i siti Web che ho trovato.

Vorrei calcolare gli angoli XYZ dati i parametri di collegamento (parametri Denavit-Hartenberg), come i seguenti :

$$\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}$$

Non capisco come trasformare questa tabella di valori nelle matrici di trasformazione appropriate necessarie per ottenere 0 T N , la posizione cartesiana e la rotazione dell'ultimo collegamento. Da lì, spero di riuscire a capire gli angoli XYZ dalla lettura del mio libro, ma qualsiasi aiuto sarebbe apprezzato.$^0T_N$

La sezione DH Matrix della pagina DH su wikipedia ha i dettagli.

Fondamentalmente si desidera utilizzare le informazioni nella tabella per creare un insieme di matrici di trasformazione omogenee. Lo facciamo perché trasformazioni omogenee possono essere moltiplicate per trovare la relazione tra frame separati da uno o più altri. Ad esempio, 0 T 1 rappresenta la trasformazione dal frame 1 al frame 0 mentre 1 T 2 rappresenta la trasformazione dal frame 2 al frame 1. Moltiplicandoli otteniamo la trasformazione dal frame 2 al frame 0, ovvero 0 T 2 = 0 T 1 1 T 2 .$^0T_1$$^1T_2$$^0T_2 = ^0T_1^1T_2$

Un modo semplice per creare ognuna delle trasformazioni è fare una trasformazione omogenea o matrice di rotazione omogenea per ogni colonna della tabella e moltiplicarle insieme. Ad esempio, la trasformazione da 1 a 0 (ad es. I - 1 T i , i = 1 ) è$^{i-1}T_i, i = 1$

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

dove

$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}$

In questo caso

$^0T1 = Rot(\theta_1)$

Una volta che hai tutte le tue trasformazioni, le moltiplichi insieme, ad es

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

$^0T_N$$d = [^0T_{N,14}, ^0T_{N,24}, ^0T_{N,34}]^T$$^0T_N$