3D Kinematics

So now were pretty good at 2D kinematics, and we can expand this to 3D kinematics.

Transformation Matrices
The derivation of these follows the derivation of the 2D transformation matrices closely; you can grind your
way through the math on your own if youd like. Since were in 3D, and since well still need
heterogeneous

x
y

matrices to perform translation, our points will now be represented with a 41 vector
z , and our
1
transformation matrices will have to be 44.
Translation

Rotation around the X axis

Rotation around the Y axis

Rotation around the Z axis

1 0 0
0 1 0
Trans( p x , py , pz ) =
0 0 1
0 0 0

1
0
0 cos()
RotX () =
0 sin()
0
0

cos()

0
RotY () =
sin()
0

cos( )
sin( )

RotZ ( ) =
0
0

0
1
0
0

px
py

pz
1

0
sin()
cos()
0

0
0

0
1

sin()
0
cos()
0

0
0

0
1

sin( )
cos( )
0
0

0 0
0 0

1 0
0 1

3D Coordinate Frames
We are going to use right-handed frames in this course. This means that if you take your right hand, put
your palm at the origin, and point your fingers along the X-axis, then curl your fingers in the direction of
the Y-axis, then your thumb will be pointed in the direction of the Z-axis. See Figure 1 for an illustration of
this.
When we talk about a rotation around an axis, that axis will remain stationary, and the other two will
rotate around it. A rotation in the positive direction means that if we look down the positive axis at the
origin, the other axes spin in a counter-clockwise direction. An example of rotating around the Y-axis in a
positive direction is shown in Figure 2.

Figure 1: Right-handed axes (from McKerrow, Introduction to Robotics)

Figure 2: Rotation around the Y-axis (from Spong et al., Robot Modeling and Control)

Assigning Frames to Joints

There are an infinite number of ways to assign coordinate frames to robotic manipulators. However, it
makes it easier for everybody to talk to each other and share results if there is a standard way of doing
these assignments. This standard way is known as the Denavit-Hartenberg Convention.
In the DH Convention, each transformation from one joint to another is a product of the following basic
transformations:
i 1

Ti = RotZ (i ) Trans(li , 0, di ) RotX (i ),

where i , li , di , i are the joint angle, link length, link offset, and link twist, respectively. These will be
discussed further.
But, an astute student might say, in moving a frame we have six degrees of freedom (three directions of
translation, three axes to rotate around), and the DH convention only gives us four parameters. How can
we represent all possible joints this way? For two adjacent coordinate frames N0 and N1 , we declare the
following two things must be true:
x1 is perpendicular to z0
x1 intersects z0
It turns out that when we limit ourselves in this way, we no longer have to transform between arbitrary
frames, but between a subset where four degrees of freedom is enough.
Were going to be dealing primarily with revolute joints, or joints with a rotation around a single axis, like
an elbow, or the rotation on a wrist. There are other kinds, of course, like a prismatic joint, which lengthens
and shortens like the arm of a crane, or a ball and socket joint like a shoulder.

On revolute joints, we will add the additional constraint on our frames that the joint will rotate around
the Z-axis.
Revolute joints which meet the above criteria can be broken down into the following three cases.
1. zi1 and zi are parallel
An example of this is a link connecting two elbow joints which turn in the same plane. We can recycle
our 2D arm for this. Figure 3 illustrates the two frames. In both frames, the Z-axis is coming out at the
reader, and so is not illustrated (the right-hand rule means the Z-axis must point this way, and not in
towards the page). 1 and l1 are both labelled. We have no twist, meaning 1 = 0. Additionally, we
require no translation along the Z-axis, so d1 = 0.

y_1

x_1
y_0

1
l_

_1

x_0

Figure 3: Illustration of z0 and z1 being parallel. Both Z-axes are pointing at the reader, and are not included.
2. zi1 and zi are not co-planar
An example of this is a link connecting two elbow joints which turn in different planes. In Figure 4,
you can see an example of this. z0 is coming directly at the viewer, and y1 is pointing exactly away
from the reader.
Referring to the DH convention, we first rotate around z0 by 1 . We then translate along the new
X-axis l1 . We do no translation along the Z-axis, so d1 = 0. We then rotate around x1 so that z1 is
pointed in the correct direction. In this case, we are looking down the X-axis at the origin, and we
rotate it /2 radians clockwise, making i = /2. This is where gets its name of a link twist.
3. zi1 and zi intersect
This is likely the trickiest case. Our arm is a good example; we have an elbow, and then we have a
wrist at the end of a link. The wrists rotation is around the center of the forearm, meaning the Z-axis
runs along the forearm as well, perpendicular to the elbows Z-axis.
Its natural to want to put the origin for our new frame in the middle of our wrist joint. However, then
theres no way for x1 to intersect z0 , and thats one of our rules for the DH convention. So instead, we
do a weird trick, and put the origin of our new frame in the same place as the old frame.
Now, where to point x1 ? Well, it needs to be perpendicular to both z1 and z0 . This leaves you with
two choices. One of them is illustrated in figure 5.
So, how does this correspond to our parameters for our transformation? Well, first we rotate around
z0 + /2 radians (look at our picture, and see why. If we had chosen for x1 to go down and to the
right, it would be /2 radians.), so thats our 1 . We then do no translation at all, so l1 = d1 = 0.
We then rotate around x1 /2 radians to get z1 pointed in the correct direction. Weird!
3

Figure 4: Illustration of z0 and z1 being not co-planar.

Figure 5: Illustration of z0 and z1 intersecting. z0 and y1 are coming out at the viewer.
We can make up the links length by adding it to l2 at the next transformation.
Another example of this type is Figure 5, but reversed, where joint 0 is a wrist, and joint 1 is an elbow.
How do you think that would work? Draw an elbow-wrist-elbow, and figure out how the frames
should be arranged.
So if those are our three examples, why is d a parameter? Whats its purpose? Well, these are just
examples, and youll find times to use it. But, also, d comes in handy when our links are not straight, easy
links; the DH convention works just as well with curvy, twisted links.

Performing 3D Kinematics
Finding a point on the end of the robot is an easy transition from 2D kinematics. For each joint i, define
i , li , di , and i , create the transformation matrices, and multiply them all up, in the correct order, and then

0
0

multiply by the point

0 . Thats it!
1
But, of course, were not interested only in the position of the end of the robot, but also the orientation,
referred to in 3D as its roll, pitch, and yaw. What were interested in is the total rotation around each of
the three axes in the original reference frame. The first thing to note is that when we assign DH values to
every joint, when all joint angles are 0, the end frame must have no rotational component when compared
to the reference frame. In other words, in its default position, the arm must have no roll, pitch, or yaw.
Sometimes, this may mean an additional set of DH parameters.
In this class, well refer to roll as the rotation around the X axis, pitch as the rotation around the Y
axis, and yaw as the rotation around the Z axis. These definitions, unfortunately, arent well standardized.
What is standardized is that these occur in the reference frame, in the order yaw, pitch, roll. Notice this is
different than how I understand it is used in reference to aircraft; in robotics, these terms refer to rotations
around the axes of the reference frame.
Lets think about the general transformation matrix in 3D. Lets shorten things like cos( ) to c .
T = Trans( p x , py , pz ) RotX () RotY () RotZ ( )

c c s c + c s s s s + c s c
s c c c + s s s c s + s s c
=
s
c s
c c
0
0
0

px
py

pz
1

So, when we have calculated our general transformation matrix, from the reference frame to the frame
at the end of the arm, we can solve for , , and by just teasing these terms out of this matrix. However,
just as in 2D, we dont trust arcsin or arccos, and only trust arctan2.
This means that before we solve for any of our angles, such as , we will solve for cos( ) and sin( ), so
that we can call arctan2(sin( ), cos( )) and avoid all numerical issues.
Consider the general transformation matrix above. Given such a matrix, we could take the 3rd row,
1st column, (hereby denoted T (3, 1)) and call arcsin on it, in order to calculate . However, we know
this isnt what we want to do. Instead, were going to multiply that number by -1, and use that term as
the first argument in our arctan2 function. This leaves us needing to calculate cos(). Using the handy
2
2
2
2
2
trigonometric identity
p that cos ( x ) + sin ( x ) = 1, we note that cos () = T (1, 1) + T (2, 1) . So, =
arctan2( T (3, 1), T (1, 1)2 + T (2, 1)2 ).
Note we could have used plus-or-minus for the 2nd argument of the above function; this will rear its
head in inverse kinematics.
Assuming 6= /2 or /2, the three angles are pulled out easily:


q
2
2
= arctan2 T (3, 1), T (1, 1) + T (2, 1)


T (2, 1) T (1, 1)
= arctan2
,
cos() cos()


T (3, 2) T (3, 3)
= arctan2
,
cos() cos()
If is /2 or /2, then we can calculate the sum or difference of and , but cannot narrow it down
further than that. So, we use a convention in which = 0.

2
=0

= arctan2( T (1, 2), T (2, 2)),

and,
=

=0
= arctan2( T (1, 2), T (2, 2)),