LAB1 : Forward and Inverse Kinematics for the PUMA 560 Manipulator

ROBOT MODELING AND CONTROL
ECE470S
LAB1 : Forward and Inverse Kinematics for the PUMA 560 Manipulator
1 Purpose
The purpose of this lab is to derive the forward and inverse kinematics equations for the PUMA 560 robot,
to implement these equations as Matlab functions, and to use these functions to perform basic computer
simulations.
2 Introduction
The PUMA (Programmable Universal Machine for Assembly) is a robot developed by Unimation in the
late 1970s for industrial automation in General Motors plants. This robot was particularly well-designed,
mechanically robust, and very reliable, so much so that it remained in production for twenty years even after
Unimation was bought out. In this lab you will familiarize yourself with the kinematics of the PUMA 560,
depicted in Figure 1. This is an articulated robot with six joints, the last three of which are a spherical
wrist.
Using the Denavit-Hartenberg (DH) convention, you will assign six frames to the six moving links of the
robot, and you will compile the associated DH table. The i-th row of the DH table contains four parameters
associated with link i:
• ai
: link length. This is the length of link i, measured along the common normal to axes zi−1 and zi
.
• αi
: link twist. This is the angle from axis zi−1 to axis zi
, measured through a rotation about axis xi
.
The sign of αi
is determined by applying the right-hand rule around axis xi
.
• di
: link offset. This is the distance between the origins of frames i−1 and i, measured along axis zi−1.
• θi
: joint angle. This is the angle between axis xi−1 and axis xi
, measured using the right-hand rule
around axis zi−1.
2.1 Forward Kinematics
With the DH table, we can compute the homogeneous transformation matrix from frame i − 1 to frame i as
H
i−1
i =




cθi − sθi
cαi
sθi
sαi aicθi
sθi
cθi
cαi − cθi
sαi aisθi
0 sαi
cαi di
0 0 0 1




Composition of these matrices gives the homogeneous transformation matrix from the base frame to link 6:
H0
6 =

R0
6
o
0
6
0 1 
= H0
1
· . . . · H5
6
.
1
2.03
76
43.18
All units in cm
43.18
38.65
15
x0
x0
y0
z0
z0
o2 = o3
o2 = o3
o2 = o3
oc
oc
oc
Figure 1: Schematics of the PUMA560 robot
By extracting the 3 × 3 matrix R0
6 and the 3 × 1 vector o
0
6
from H0
6
, the forward kinematics computation
is complete. Recall the geometric meaning of R0
6 and o
0
6
: the columns of R0
6 are the unit axes of the end
effector frame 6 represented in the coordinates of frame 0: R0
6 = [x
0
6
| y
0
6
| z
0
6
]; the vector o
0
n
is the origin of
frame 6 expressed in the coordinates of frame 0.
2.2 Inverse Kinematics
Given a desired position o
0
d ∈ R
3 and desired orientation Rd ∈ SO(3) of the end effector, the inverse
kinematics problem is to find (θ1, . . . , θ6) such that R0
6
(θ1, . . . , θ6) = Rd and o
0
6
(θ1, . . . , θ6) = o
0
d
.
Since the PUMA 560 has six links and a spherical wrist, one can solve the inverse kinematics problem by
the technique of kinematic decoupling. In kinematic decoupling, the problem is divided in two parts: inverse
position and inverse orientation.
Inverse position. The position of the wrist centre oc only depends on the angles of the first three joints,
(θ1, θ2, θ3). The idea is to compute the desired location of the wrist centre and then find (θ1, θ2, θ3) accordingly.
• Compute the vector
o
0
d − Rd


0
0
d6

 .
2
• Find (θ1, θ2, θ3) such that o
0
c
(θ1, θ2, θ3) =


xc
yc
zc

 = o
0
d − Rd


0
0
d6

.
Inverse orientation. We need to impose that R0
6 = Rd. Having computed (θ1, θ2, θ3), we can compute the
rotation matrix R0
3
(θ1, θ2, θ3). Using the fact that R0
6 = R0
3R3
6
, we need to impose that
R
0
3
(θ1, θ2, θ3)R
3
6
(θ4, θ5, θ6) = Rd.
Multiplying both sides by (R0
3
)
⊤ we obtain
R
3
6
(θ4, θ5, θ6) = (R
0
3
)
⊤Rd.
This equation must be solved for (θ4, θ5, θ6). This is easy because, as we have seen in class, the angles
(θ4, θ5, θ6) are just the ZYZ Euler angle coordinates of the rotation matrix (R0
3
)
⊤Rd (when the x4 axis is
chosen as x4 = −z3 × z4).
In summary, the procedure to find (θ4, θ5, θ6) is this.
• Compute H0
3
(θ1, θ2, θ3) = H0
1
(θ1)H1
2
(θ2)H2
3
(θ3), and extract the rotation matrix R0
3
.
• Compute (R0
3
)
⊤Rd.
• Find the ZYZ Euler angles coordinates (θ, φ, ψ) of (R0
3
)
⊤Rd by using the equations given in class, or
equations (2.28), (2.30), (2.31) (or (2.29), (2.32), (2.33)) of the textbook, assuming that the x4 axis
has been chosen as x4 = −z3 × z4. Please note that the equations in the textbook use the convention
atan2(x, y), whereas in Matlab (and in class), the convention atan2(y, x) is adopted. To avoid errors,
refer to the equations presented in class.
• Set θ4 = φ, θ4 = θ, θ6 = ψ.
3 Preparation
1. Print out page 2 of this document and draw frames 0, 1, 2, 3 on the three-dimensional schematic of
the PUMA 560. Place frame 0 at the base of the robot. Assign frames 1,2,3 according to the DH
convention. In particular, your assignment of frame 3 should be consistent with the presence of a
spherical wrist (not represented in the figure), so that the z3 axis should run parallel to the length of
the last link. Place the origins of frames 2 and 3 in the location indicated on the PUMA schematic.
2. Based on your frame assignment, and the lengths marked on Figure 1, write down the 6 × 4 DH table
of the PUMA 560 robot, assuming that the wrist length is d6 = 20cm and that x4 = −z3 × z4.
3. Write down closed-form expressions of the inverse kinematics of the PUMA 560. Specifically, you
should write (θ1, . . . , θ6) as functions of the entries Rd and o
0
d
. Please note the position of the wrist
centre oc marked on the PUMA schematic.
4 Experiment
In this lab you will write Matlab functions implementing the forward and inverse kinematics of the PUMA
560. These functions will be used in all subsequent labs of this course, so it is important that you test them
accurately. You will use the robotics toolbox to define and plot the robot configuration in three-space.
3
4.1 Definition of robot structure
Write a function mypuma560.m that defines a new robot structure with the PUMA 560 parameters you
determined in your preparation. Your function must have these arguments
myrobot = mypuma560(DH)
where DH is a 6 × 4 matrix of DH parameters when the robot is in its rest position (i.e., when all θi
’s are
zero). The i-th row of the matrix DH will contain the parameters [θi di ai αi
] in this order.
To create the robot structure myrobot, your function should use the commands Link and SerialLink
provided by the Robotics Toolbox. See the Robotics Toolbox help to see how these commands work. The
Robotics Toolbox manual is the file robot.pdf posted on Blackboard.
4.2 Plot a sample joint space trajectory
Now you will generate a sequence of 200 joint angles in a 200 × 6 matrix q defined as follows: the i-th row
of q contains the values of joint angles θ1, . . . , θ6 at step i. The values of the joint angles should be selected
as follows:
• θ1 should transition from 0 to π in 200 steps.
• θ2 should transition from 0 to π/2 in 200 steps.
• θ3 should transition from 0 to π in 200 steps.
• θ4 should transition from π/4 to 3π/4 in 200 steps.
• θ5 should transition from − π/3 to π/3 in 200 steps.
• θ6 should transition from 0 to 2π in 200 steps.
To generate each row θi
, use the Matlab command theta=linspace(initial,final,N).
Plot the robot evolution corresponding to the joint angles above by issuing the command
plot(myrobot,q)
Observe the geometric representation of the robot. The plot command plots the minimum amount of
information needed to represent the motion of the robot: namely, the origin of the coordinate frames and
segments connecting them, but it does not represent detailed geometric structure of the robot such as the
fact that some links are L-shaped.
4.3 Forward Kinematics
Develop a function forward.m
H = forward(joint,myrobot)
where:
• joint is a 6 × 1 vector containing the values of the six joint angles.
• myrobot is the robot structure generated by your function mypuma560.
• Your function must return H, the homogeneous transformation matrix H0
6 = A1(θ1) · . . . · A6(θ6).
4
Note that H will encode information about the position and orientation of the end effector. Specifically,
the orientation of the end effector with respect to the base frame is represented by the rotation matrix
H(1:3,1:3), while the coordinates of the origin of the end effector are given by H(1:3,4).
As a test, consider again the matrix q defined in Section 4.2. Using the function forward, for each row of q
compute the coordinates of the end effector. Store all these coordinates in a 200 × 3 matrix named o. Then,
plot the trajectory o(t) with red colour:
>> plot3(o(:,1),o(:,2),o(:,3),’r’)
>> hold on
Now issue the command
>> plot(myrobot,q)
and verify that the origin of the end effector traces out precisely the red trajectory you created using your
forward function.
4.4 Inverse Kinematics
Develop a function inverse.m
q = inverse(H,myrobot)
where:
• H is the 4 × 4 homogeneous transformation matrix containing the desired position and orientation of
the end effector.
• myrobot is the robot structure generated by your function mypuma560.
• Your function must return q, the 1 × 6 vector of joint angles solving the inverse kinematics problem.
It’s very important that you get this function working properly. You’ll use it in lab 3. Test your function as
follows: when H is given as (units are in metres)
H = [cos(pi/4) -sin(pi/4) 0 20; sin(pi/4) cos(pi/4) 0 23; 0 0 1 15; 0 0 0 1]
your inverse function should return
q = [ -0.0331 -1.0667 1.0283 3.1416 3.1032 0.8185]
Next, suppose we want the PUMA to pick up an object on the table at (x, y, z) = (10, 23, 15) (units in
centimetres), then transport the object to (x, y, z) = (30, 30, 100). Meanwhile, we want the end effector to
have a constant orientation defined by a constant matrix R. Keep in mind that the inverse kinematics are
not unique. To overcome this, it may be useful to pick a solution (e.g. left-arm, elbow-up) and develop
your inverse function with this convention. Note also that your solution contains the square root operator
which might return a complex number with zero real part. Use the command real(sqrt(...)) to avoid
complications.
1. Define a 100 × 3 matrix d like this: every row of o contains a desired position of the end effector,
(x, y, z). The sequence of end effector positions must start at (10, 23, 15), end at (30, 30, 100), traveling
along a straight line.
2. Define a 3 × 3 matrix R to be the rotation matrix Rz,π/4. This matrix defines the desired constant
orientation of the end effector throughout the pick-and-place operation.
5
3. For each row d, use your function inverse to solve the inverse kinematics problem (i.e., use d(i,:)
and R defined earlier in the inverse kinematics problem). Store the result as a row of a matrix q. At
the end of this iteration, you will have a 100 × 6 matrix q of joint angles.
4. Following the procedure of Section 4.3, plot the configuration of the PUMA corresponding to the
sequence of joint angles you’ve just found, and verify that the end effector has constant orientation,
and that its origin traces out the straight line from (10, 23, 15) to (30, 30, 100).
5 Submission
Each student will submit on Quercus a preparation report and a zipped folder containing your Matlab
functions. Thoroughly comment your code. Your folder may contain intermediate Matlab functions, but
calls to the main functions forward and inverse must work without any modification required. The main
file in the folder should be called Lab1.m, and it should work out the various steps in Sections 4.1-4.4, clearly
displaying the main steps.
6