How do I convert link parameters and angles (in kinematics) into transformation matrices in programming logic?

Question

I'm doing robotics research as an undergraduate, and I understand the conceptual math for the most part; however, when it comes to actually implementing code to calculate the forward kinematics for my robot, I am stuck. I'm just not getting the way the book or websites I've found explain it.

I would like to calculate the X-Y-Z angles given the link parameters (Denavit-Hartenberg parameters), such as the following:

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

I don't understand how to turn this table of values into the proper transformation matrices needed to get $^0T_N$, the Cartesian position and rotation of the last link. From there, I'm hoping I can figure out the X-Y-Z angle(s) from reading my book, but any help would be appreciated.

Answer 1

The DH Matrix section of the DH page on wikipedia has the details.

Basically you want to use the information in your table to create a set of homogeneous transformation matrices. We do so because homogeneous transformations can be multiplied to find the relation between frames seperated by one or more others. For example, $^0T_1$ represents the transformation from frame 1 to frame 0 while $^1T_2$ represents the transformation from frame 2 to frame 1. By multiplying them we get the transformation from frame 2 to frame 0, i.e. $^0T_2 = ^0T_1^1T_2$.

An easy way to create each of the transformations is to make a homogeneous transformation or homogeneous rotation matrix for each column in the table and multiply them together. For example, the transformation from 1 to 0 (e.g. $^{i-1}T_i, i = 1$) is

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

where

$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 this case

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

Once you have all your transformations you multiply them togther, e.g.

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

Finally you can read the displacement vector out of the homogenous transform $^0T_N$ (i.e. $d = [^0T_{N,14}, ^0T_{N,24}, ^0T_{N,34}]^T$). Similarly you can read out the rotation matrix from $^0T_N$ to find the X-Y-Z angles.