Comment convertir les paramètres de liaison et les angles (en cinématique) en matrices de transformation dans la logique de programmation?

Je fais de la recherche en robotique en tant que premier cycle et je comprends les mathématiques conceptuelles pour la plupart; cependant, quand il s'agit d'implémenter du code pour calculer la cinématique avant de mon robot, je suis bloqué. Je ne comprends tout simplement pas comment le livre ou les sites Web que j'ai trouvés l'expliquent.

Je voudrais calculer les angles XYZ en fonction des paramètres de liaison (paramètres Denavit-Hartenberg), tels que les suivants :

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

$^0T_N$

La section DH Matrix de la page DH sur wikipedia contient les détails.

$^0T_1$$^1T_2$$^0T_2 = ^0T_1^1T_2$

$^{i-1}T_i, i = 1$

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

où

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

Dans ce cas

$^0T1 = Rot(\theta_1)$

Une fois que vous avez toutes vos transformations, vous les multipliez ensemble, par exemple

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