Kevin Walchko Created: 10 July 2017
Denavit Hartenberg (DH) is an attempt to standardize how we represent serial manipulators (i.e., robot arms). It is typically one of the first ways you learn. It is really easy (methodical) to do forward kinematics, but becomes more challenging when doing inverse kinematics. Here we are going to introduce what is goning on, but you need to focus on the DH process. If you follow the process, then all will work out fine. Don't get too hung up on the begining math, understand the concepts so you can follow the DH process.
In [1]:
%matplotlib inline
from __future__ import print_function
from __future__ import division
import numpy as np
from math import cos, sin, pi
from IPython.display import HTML # need this for embedding a movie in an iframe
Robot arms and legs are hard to control (lots of math), but are required for most robotic applications.
DARPA Orbital Express, was a space robotics demo to perform on-orbit survicing (repair, refueling, etc). This demo proved if we build modular satellites, we can extend the on-orbit life.
EOD robots used to neutralize IEDs. Their arm is equiped with a variety of payloads to handle various types of IEDs.
Prosthetic limbs in the olden days werew a wooden peg to replace your leg. Today, robotic arms and legs are becomming fully functional replacements, controlled via neural impulses from the person. Although people might tie these robotic limbs to the rise of cyborgs, but depending on your definition of cyborg, you could also call a person with an implanted pacemaker a cyborg (see definition below).
Cyborg: [noun] a person whose physiological functioning is aided by or dependent upon a mechanical or electronic device. ref
Kinematics is the study of motion without regard to the forces which cause it. Kinematics of manipulators involves the study of the geometric and time based properties of the motion, and in particular how the various links move with respect to one another and with time.
Pose: The combination of position and orientation of a frame relative to a reference or inertial frame. Think Zoolander on a fashion shoot, "strike a pose"
This lesson will talk about matrix and vector operations. A nice review of the various mathematical operators is wikipedia. Take a look at how you multiply 2 matrices together (Matrix Product (two matrices)) and it will give you an idea of how it works. Ultimately we will use numpy
to do these operations for us.
We want to describe positions and orientations of bodies in space relative to a reference (or in some cases an inertial) frame. So if we wanted to know where something was, we could define it from this coordinate frame as $[x,y,z]$.
We are going to make this more complex and define multiple coordinate frames, which will all be rotated in some fashion, and try to determine a point (maybe the end-effector of a robot arm) relative to a base (inertial) reference frame.
There are 2 types of manipulators in robotics: serial and parallel. They are used in different applications for manufacturing.
Definitions:
For most robotic applications, we need to be able to calculate both the forward and reverse.
Type | Pro | Con |
---|---|---|
Serial | easy forward kinematics | complex inverse kinematics |
Parallel | easy inverse kinematics | complex forward kinematics |
For this class we are going to focus on serial manipulators.
An example (sort of) is shown below. Notice the real robot is represented as a simpler drawing of little metal bars:
The KR270 has 6 joints and is a standard industrial type robot for manufacturing. Notice again, the wrist, is represented as 2 revolute joints co-located (on top of each other) which is not realistic.
A rotation matrix transforms (or rotates) a point from one location to another. If we start off simple and look at a 2D matrix:
$$ R = \begin{bmatrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta) \end{bmatrix} $$If I have a 2D point located at $v = [x,y]$ and I want to rotate it (or turn it in the 2D plane), I can do that with this matrix as $v' = Rv$ where $v'$ is the new 2D position.
If we expand this to 3D, so our matrix would be a 3x3, we can start to do things like this:
Now, please note, the beginning of this gif
starts off with a translation (or movement), the does a rotation as the cube spins around. Another great example, is your arm! If you rotate your elbow, your hand moves. You can define that rotation my a matrix operation. Also note, we are not changing the length or size of anything when we do a rotation. We are just moving something (usually in some sort of circular/arc fashion) from one place to another. Again, when you rotate your arm, it doesn't change size does it? If it does, go see a doctor!
A nice thing about rotation matricies, is:
$$ R^B_A = (R^A_B)^T = (R^A_B)^{-1} $$This is because they are orthonormal (the dot product of the x, y, and z components is 0). This means a matrix inverse and matrix transpose produce the same results, which is good since matrix inverse is CPU intensive compared to a matrix transpose (shown below).
$$ R = \begin{bmatrix} 1 & 2 & 3 \\ 4 & 5 & 6 \\ 7 & 8 & 9 \end{bmatrix} \\ R^T = \begin{bmatrix} 1 & 4 & 7 \\ 2 & 5 & 8 \\ 3 & 6 & 9 \end{bmatrix} = inv(R) = R^{-1} $$Notice a transpose just turns matrix columns into matrix rows ... no math!
Note: the magnitude of the rows and columns of a real rotation matrix are 1. The example above is sloppy in that fact. I was lazy and just wanted to remind you how transpose worked.
In [2]:
# rotation example
# euler angles: 0 0 0
Rab = np.eye(3) # so this rotation transforms from b to a
Pb = np.array([0,0,1])
Pa = Rab.dot(Pb)
print('Original position', Pb)
print('\nRotation:\n', Rab)
print('\nNew orientation:', Pa)
In [3]:
# rotate 45 deg about x-axis
Rab = np.array([[1,0,0], [0,cos(pi/4), -sin(pi/4)], [0,sin(pi/4),cos(pi/4)]])
Pb = np.array([0,0,1])
Pa = Rab.dot(Pb)
print('Original position', Pb)
print('\nRotation:\n', Rab)
print('\nNew orientation:', Pa)
You should aready be familar with vectors from other courses. We are only going to deal with 2D (in a plane: x, y) or 3D (x, y, z) vectors for our position. We are going to have to reference the position of an object relative to multiple referenece frames. This section is going to lay the basic mathematical foundation.
Now the position/orientation, or pose, of {B} relative to {A} is:
$$ P^A = R^A_B P^B $$Now ultimately, we want to know the location of a point in {B} relative to an inertial frame (say the base of our robot arm) in {A}.
What the final equation means is the position of $P^B$ in frame A is equal to the offset between {A} and {B} (i.e., $P^A_B$) plus the change in orientation between {A} and {B} (i.e., $R^A_B P^B$).
In [4]:
# rotate 45 deg about x-axis
# add a translation in now
Rab = np.array([[1,0,0], [0,cos(pi/4), -sin(pi/4)], [0,sin(pi/4),cos(pi/4)]])
Pb = np.array([0,0,1])
Pab = np.array([3,0,0]) # {b} is 3 units infront of {a}
Pa = Pab + Rab.dot(Pb)
print('Original position', Pb)
print('\nRotation:\n', Rab)
print('\nTranslation:\n', Pab)
print('\nNew position:', Pa)
This is typically not how we do the math. Instead, robotics combines these opertions together in a form that becomes easier to program. A compact representation of the translation and rotation is known as the Homogeneous Transformation. This allows us to combine the rotation ($R^A_B$) and translation ($P^A_B$) of the general transform in a single matrix form.
$$ T^A_B = \begin{bmatrix} & R^A_B & & P^A_B \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ \begin{bmatrix} P^A \\ 1 \end{bmatrix} = T^A_B \begin{bmatrix} P^B \\ 1 \end{bmatrix} $$This compact notation allows us to use numpy
or matlab
to write series of equations as matricies and do standard matrix operations on them. Now, as we attach frames to serial manipulator links, we will be able to combine these matricies together to calculate where the end effector is.
In [5]:
# Now let's do the combined homogenious matrix and see if we get the same answer
# rotate 45 deg about x-axis
# add a translation in now
Tab = np.array([[1,0,0,3], [0,cos(pi/4), -sin(pi/4),0], [0,sin(pi/4),cos(pi/4),0],[0,0,0,1]])
Pb = np.array([0,0,1,1])
Pa = Tab.dot(Pb)
print('Original position', Pb)
print('\nRotation and translation:\n', Tab)
print('\nNew position:', Pa)
Now that we have a basic understanding of translation and rotation, we can look at a process (for serial manipulators) to automate it. We will use the DH method to develop the symbolic equations and use python sympy
to simplify them so we can program the equations.
Now the process laid out in Craig's book, defines some parameters for eash link in a robot arm:
$a_i$ | link length | distance from $z_i$ to $z_{i+1}$ measured along $x_i$ |
$d_i$ | offset | distance from $x_{i-1}$ to $x_i$ along $z_i$ |
$\alpha_i$ | twist | angle from $z_i$ to $z_{i+1}$ measured about $x_i$ |
$\theta_i$ | rotation | angle from $x_{i−1}$ to $x_i$ measured about $z_i$ |
Note: on a quiz or GR I will give you these definitions, I don't even memorize them. But know the process below
Summary of steps:
Now, once you have the parameters, you can enter them into the followng matrix to get the relationship between frame i and frame i+1. Note, that these are not euler angles, but rather:
This sequence turns into the the following homogenious transform:
$$ \begin{eqnarray} T^{i-1}_i = R_x(\alpha_{i-1}) D_x(a_{i-1}) R_z(\theta_i) D_z(d_i) \\ \\ T^{i-1}_i = \begin{bmatrix} \cos(\theta_i) & -\sin(\theta_i) & 0 & a_{i-1} \\ \sin(\theta_i)\cos(\alpha_{i-1}) & \cos(\theta_i)\cos(\alpha_{i-1}) & -\sin(\alpha_{i-1}) & -\sin(\alpha_{i-1})d_i \\ \sin(\theta_i)\sin(\alpha_{i-1}) & \cos(\theta_i)\sin(\alpha_{i-1}) & \cos(\alpha_{i-1}) & \cos(\alpha_{i-1})d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{eqnarray} $$You will create 1 matrix for each link in your serial manipulator. Then you can multiply these matricies together to get the transform from base frame {0} to end effector frame {3} by: $T^0_3 = T^0_1 T^1_2 T^2_3$
Also, the general format of every homogenious matrix is:
$$ T_{4x4} = \begin{bmatrix} R_{3x3} & t_{3x1} \\ \begin{bmatrix} 0 & 0 & 0 \end{bmatrix} & 1 \end{bmatrix} $$where $R$ is your rotation matrix, $t$ is your translation, and T is your homogenious matrix. Note, later when we do computer vision, you will see the homogenious matrix refered to as $H$. Basically no one has agreed to what variables are called across different fields of study. So be flexible and try to recognize things for what they are ... you will see these again.
In [6]:
HTML('<iframe src="https://player.vimeo.com/video/238147402" width="640" height="360" frameborder="0" webkitallowfullscreen mozallowfullscreen allowfullscreen></iframe><p><a href="https://vimeo.com/238147402">Denavit–Hartenberg parameters</a> from <a href="https://vimeo.com/user59907133">kevin</a> on <a href="https://vimeo.com">Vimeo</a>.</p>')
Out[6]:
Following the process for assigning frames to a manipulator, you get the following:
Looking at the above frames, they are related by:
Link | $a_{i-1}$ | $\alpha_{i-1}$ | $d_i$ | $\theta_i$ |
---|---|---|---|---|
1 | 0 | 0 | 0 | $\theta_1$ |
2 | $a_1$ | 0 | 0 | $\theta_2$ |
3 | $a_2$ | 0 | 0 | 0 |
Now using Craig eqn 3.6, we can substitute these values in and get the relationship between the inertial frame and the end effector. However note, $\theta_i$ are variable parameters. Typically we would simplify these equations down leaving only the $\theta_i$ parameters. Let's use the python symbolic toolbox to generate the equations of motion.
In [7]:
# Let's grab some libraries to help us manipulate symbolic equations
import sympy
from sympy import symbols, sin, cos, pi, simplify
def makeT(a, alpha, d, theta):
# create a modified DH homogenious matrix
# this is the same matrix as above
return np.array([
[ cos(theta), -sin(theta), 0, a],
[sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -d*sin(alpha)],
[sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), d*cos(alpha)],
[ 0, 0, 0, 1]
])
def simplifyT(tt):
"""
This goes through each element of a matrix and tries to simplify it.
"""
ret = tt.copy()
for i, row in enumerate(tt):
for j, col in enumerate(row):
ret[i,j] = simplify(col)
return ret
def subs(tt, m):
"""
This allows you to simplify the trigonomic mess that kinematics can
create and also substitute in some inputs in the process
Yes, this is basically the same as above. I could combine these into 1
function, but I wanted to beclearer on what I am doing.
"""
ret = tt.copy()
for i, row in enumerate(tt):
for j, col in enumerate(row):
try:
ret[i,j] = col.subs(m)
except:
ret[i,j] = simplify(col)
return ret
In [9]:
# make thetas (t) and link lengths (a) symbolics
t1, t2 = symbols('t1 t2')
a1, a2 = symbols('a1 a2')
# let's create our matrices
T1 = makeT(0, 0, 0, t1)
T2 = makeT(a1, 0, 0, t2)
T3 = makeT(a2, 0, 0, 0)
# T13 = T1 * T2 * T3
T13 = T1.dot(T2.dot(T3))
print('T1 = ', T1)
print('T2 = ', T2)
print('T3 = ', T3)
print('\nSo the combined homogenious matrix is:\n')
print('T13 = ', T13)
So that looks a little messy with all of the sines and cosines ... let's use the python symbolic capabilities in sympy
to help us reduce this a little and figure out where the end-effector is relative to the base (e.g. inertial frame).
In [10]:
ans = simplifyT(T13)
print(ans)
print('-'*25)
print('position x: {}'.format(ans[0,3]))
print('position y: {}'.format(ans[1,3]))
Later, we will derive the same 2 link manipulator a different way and come up with the same equations for position of the end-effector in the x, y plane. For simple manipulators, DH is overkill, however, for most real manipulators (remember the DARPA videos), it is useful to understand what is going on if you do robotics.
In [11]:
# what if I wanted to substitute in an angle?
# just give it an array of tuples
ans = subs(T13, [(t1, 0)]) # here it is only t1, but I could do: [(t1, angle), (t2, angle)]
print(ans)
print('-'*25)
print('position x: {}'.format(ans[0,3]))
print('position y: {}'.format(ans[1,3]))
Now following the steps above, we get the complete DH table below. Notice, the distances in the picture (d) have been changed to m, because I didn't want people getting confused between distance d and DH parameter d. Therefore everything is now distance m to remove confusion:
Link | $a_{i-1}$ | $\alpha_{i-1}$ | $d_i$ | $\theta_i$ |
---|---|---|---|---|
1 | 0 | 0 | $m_1$ | $\theta_1$ |
2 | $m_2$ | 90 | 0 | $\theta_2$ |
3 | $m_3$ | 0 | 0 | $\theta_3$ |
4 | $m_5$ | -90 | $m_4$ | $\theta_4$ |
5 | 0 | 90 | 0 | $\theta_5$ |
6 | 0 | -90 | $m_6$ | $\theta_6$ |
A step-by-step walk through is found here (this is included in the zip file and called lsn7-extra.pdf
), make sure you understand this process because you will be tested on it. Also, I strongly suggest you do not wait until the night before the test to learn this!
Practice makes perfect. Try to do the following from Craig so you will have success on the GR.
I guarantee there will be a DH problem on the GR where you have to draw frames or fill out the table or something, so please practice it.
This work is licensed under a Creative Commons Attribution-ShareAlike 4.0 International License.