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

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.

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.

• Wouldn't alpha_2 be -90 degrees? – Grace Mar 18 '13 at 7:18