プログラミングロジックでリンクのパラメータと角度（運動学）を変換行列に変換するにはどうすればよいですか？

私は学部生としてロボット工学の研究を行っており、概念的な数学の大部分を理解しています。しかし、ロボットのフォワードキネマティクスを計算するコードを実際に実装することになると、行き詰まっています。私が見つけた本やウェブサイトがそれを説明する方法を単に得ていません。

次のようなリンクパラメーター（Denavit-Hartenbergパラメーター）を指定して、XYZ角度を計算したいと思います。

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

ウィキペディアのDHページのDHマトリックスセクションに詳細があります。

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

どこ

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

この場合

$^0T1 = Rot(\theta_1)$

すべての変換が完了したら、それらを一緒に乗算します。たとえば、

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