ROBOT MODELING AND CONTROL

ECE470S

LAB2 : Forward and Inverse Kinematics for the KUKA Robotic Arm

1 Purpose

The purpose of this lab is to adapt the results of lab 1 for implementation in a real robot. The objective is

to make the robot arm draw on paper.

2 Introduction

The KUKA robotic arm depicted in Figure 1 is an articulated manipulator with a three-degree-of-freedom

wrist and a gripper. Highlighted in Figure 1 are the axes of rotation of joints 1 to 6. A pencil, not displayed

Figure 1: The KUKA robotic arm in the HOME configuration

in Figure 1, will be attached to the gripper and will point along the pencil heading direction indicated in the

figure. The tip of the pencil is the end effector of the robot. The objective of this lab is to make the robot

draw patterns on paper.

The kinematic parameters of the robot are detailed in Figure 2. The red elements in the figure are the joint

axes. The values of various parameters are found in the table below.

d1 400 mm

a1 25 mm

a2 315 mm

a3 35 mm

d4 365 mm

a6 296.23 mm

d6 161.44 mm

1

pencil heading

pencil tip

pencil tip

Figure 2: Schematic diagrams illustrating the KUKA kinematic parameters

Figure 3: Schematic diagram of KUKA robot arm not at rest.

2

3 Preparation

Please submit a complete preparation at the beginning of the lab session. Prior to the lab, you should

make sure that step 4 of the preparation returns consistent results. An incomplete or incorrect preparation

will be penalized.

1. Figure 3 depicts the robot in a generic configuration away from the rest position. Print out this

page and draw on the figure coordinate frames o1x1y1z1, o2x2y2z2, o3x3y3z3, o4x4y4z4 and o5x5y5z5

according to the DH convention, and write the DH table of the robot. In particular, choose the x1 axis

parallel to the x0 axis, x1 = x0 (this is done for convenience).

If your DH frame assignment is correct, you should find that when θ1 = θ2 = θ3 = 0, link 2 is parallel

to the ground, and link 3 is vertical, pointing downward. A schematic diagram of the KUKA robot

arm in the HOME configuration is illustrated in Figure 4 which you should verify corresponds to

(θ1, θ2, θ3, θ4, θ5, θ6) = (0, π/2, 0, 0, π/2, 0).

Figure 4: The KUKA robotic arm schematic diagram in the HOME configuration

2. Derive the inverse kinematics of the robot. Specifically, given R0

6

(θ1, . . . , θ6) = Rd and o

0

6

(θ1, . . . , θ6) =

o

0

d

, find (θ1, θ2, θ3, θ4, θ5, θ6). You will find two solutions: elbow up and elbow down. Find the elbow

up solution. Write your derivations neatly on paper. As in lab 1, one can solve the inverse kinematics

problem by the technique of kinematic decoupling in which the problem is divided in two parts: inverse

position and inverse orientation.

• The position of the wrist centre oc is shown in Figure 2. First find (θ1, θ2, θ3) such that o

0

c

(θ1, θ2, θ3) =

xc

yc

zc

= o

0

d − Rd

− a6

0

d6

.

• Then solve the equation

R

3

6

(θ4, θ5, θ6) = (R

0

3

)

⊤Rd.

3

for (θ4, θ5, θ6).

3. Modify your inverse kinematics function from lab 1 to incorporate the changes of this setup. Specifically,

write Matlab functions mykuka.m, forward kuka.m, and inverse kuka.m as follows.

myrobot = mykuka(DH) defines the robot structure of the KUKA robot with the 6 × 4 DH table you

found earlier.

H = forward kuka(q,myrobot) returns the homogeneous transformation matrix H of the end effector,

where q is the 6 × 1 vector of joint angles, and myrobot is the robot structure defined above.

q = inverse kuka(H,myrobot) returns the 6×1 vector of joint angles q = (θ1, θ2, θ3, θ4, θ5, θ6), where

H is the 4 × 4 homogeneous transformation matrix H =

Rd o

0

d

0 1

.

4. Test your software: you should get

>> kuka=mykuka(DH);

>> forward_kuka([pi/5 pi/3 -pi/4 pi/4 pi/3 pi/4]’,kuka)

ans =

0.1173 -0.3109 0.9432 368.9562

-0.8419 -0.5349 -0.0717 420.4832

0.5268 -0.7856 -0.3245 120.8570

0 0 0 1.0000

>> inverse_kuka(ans,kuka)

ans =

0.6283

1.0472

-0.7854

0.7854

1.0472

0.7854

4 Experiment

In this lab you will learn to interface the KUKA robot with Matlab, you will then calibrate the DH parameters

by taking measurements, and finally you will use the Matlab functions you developed in your preparation to

make the robot draw patterns on paper. We begin by recalling the Matlab commands that you can use to

control the Kuka robot arm.

4.1 Safety procedures and manual operation of the robot

The KUKA robot you’ll use in this course must be handled with great care, placing particular attention

to issues of safety. Before you begin this lab, you need to have read Sections 1-4 of the document titled

Intro KUKA workstation, and reviewed the safety checklist document.

Confirm with your lab TA that you have understood the safety procedures and that you have read the document Intro KUKA workstation. Now perform the hands-on activity 1 in Section 4 of Intro KUKA workstation.

Before proceeding to the next section, show the TA what you have learned.

4

4.2 Review: Commanding the KUKA robot arm through Matlab

Below are the Matlab commands you will use to control the Kuka robot arm.

• startConnection establishes connection between Matlab and KUKA.

• stopConnection terminates connection between Matlab and KUKA.

• getAngles() returns the vector of current joint angles q = (θ1, θ2, θ3, θ4, θ5, θ6).

• stop() stops KUKA motion.

• moveAxis(axis,vel) moves a single KUKA axis. axis takes a value between 1 and 6 that corresponds

to the joint angle to be commanded. vel is a signed value that determines the commanded angular

speed of the joint. In this lab vel should be set to no greater than 0.01.

• setAngles(q,vel) sets KUKA arm angles to those defined in q to within a small tolerance. vel

corresponds to the speed of motion. For this lab, set vel to 0.04. To cancel the command during

execution press ctrl c and run stop() in the Matlab command window.

• setHome(vel) sets KUKA arm to the HOME configuration in Figure 1. vel corresponds to the speed

of motion. For this lab, set vel to 0.04. To cancel the command during execution press ctrl c and

run stop() in the Matlab command window.

• setGripper(state) sets the gripper state. setGripper(0) closes the gripper; setgripper(1) opens

the gripper.

Warning: Never use the clear all command in Matlab. Instead use clearvars -except udpObj

to avoid errors from occurring.

The steps to connect KUKA to the external PC:

• Run startConnection in Matlab.

• On the KUKA SmartPad run RSI Ethernet.src until the line RSI MOVECORR().

• To run the program, hold down half-way one of the enable buttons on the back of the KUKA SmartPAD.

The enable buttons are labelled 3 and 5 in Figure 5. If you press too hard, an emergency stop will

be activated. While holding down the enable button, press and hold the run button labelled 10 in

Figure 6. Note that when running the line RSI ON a warning will appear saying ‘Caution – sensor

correction is activated”. Simply confirm this warning to continue.

The steps to command the KUKA arm from Matlab are as follows:

• Run the desired command in the Matlab command window. The robot will not yet move.

• Hold down half-way one of the enable buttons on the back of the KUKA SmartPAD.

• While holding down the enable button, press and hold the run button. Now the robot will carry out

the desired command as long as both the enable and run buttons are being held down.

• To make the robot stop moving, simply release the enable button. To cancel the current command,

press ctrl c and run stop().

• If the robot ever hits an object while moving (such as the ground), immediately release the enable

button and the robot will stop. Then press ctrl c in the Matlab control window to cancel the

command and run stop(). Immediately call a TA to resolve the collision.

5

Figure 5: 3 and 5 are Enable buttons on the KUKA SmartPAD.

• If the robot does not move when commanded, it means an error has occurred. Call a TA to resolve

the issue.

• For safety, students must never open the safety gate enclosing the robot unless instructed to do so.

The enable and start button must never be pressed while the safety gate is open.

Warning: Make sure to FIRST connect KUKA to Matlab using startConnection.m. THEN

run the RSI from the Kuka SmartPad

The steps to disconnect KUKA from the external PC:

• When finished running the lab, return to the program RSI Ethernet.src on the SmartPAD, click on

line 32 and then click Block selection to set the program cursor to the command ret=RSI OFF().

Then continue running the program to the end. The program status indicator will turn black to

indicate the program is complete.

• Run stopConnection in Matlab.

The steps to deal with an RSI connection problem:

• Type CTRL+C within the Matlab environment.

• There are two options available on the top portion of the SmartPAD screen: cancel or reset. Use

reset, not cancel, to reinitialize the RSI program from the Kuka SmartPAD.

• After resetting the RSI program, push the play button multiple times until the program reaches the

RSI MOVECORR() line.

• In Matlab, run getAngles() to verify that the RSI connection has been correctly re-established.

6

Figure 6: 10 is the Run button on the KUKA SmartPAD.

4.3 Calibration of DH parameters

The DH parameters provided to you in the preparation were approximate. You need to calibrate them in

order to improve the accuracy of the forward kinematics function. The main source of the inaccuracy are

the parameters a6, and d6. We will tune these using a simple optimization approach. These are the steps

we will perform to calibrate the robot:

• Data collection. We will collect data pairs (Qi

, Xi) for three sample points, where Qi

is the joint

vector of the robot, and Xi

is the corresponding position of the pencil tip in frame 0.

• Parametric robot structure. We will parametrize the robot structure definition in mykuka.m by

two parameters, δ1 and δ2, which will perturb the DH parameters a6 and d6. The parameters δ1 and

δ2 are placed in a 2 × 1 vector delta.

• Cost function. We will define a cost function deltajoint.m (provided to you) that, given the vector

delta of DH parameter perturbations, uses forward kinematics to predict coordinates Xˆ

i of the end

effector corresponding to the joint vectors Qi measured in step 1. The cost measures the discrepancy

between the predictions Xˆ

i and the actual Xi measured in step 1.

• Cost minimization. Using the Matlab command fminunc.m, we will find the vector delta minimizing

the above cost function. This vector constitutes the perturbations to the parameters a6 and d6 that

minimize the discrepancy between the measured Xi and the Xˆ

i predicted using forward kinematics.

• Calibrated robot structure. Using delta found in the cost minimization step, we will define a new,

calibrated robot structure.

Now the detailed steps you need to perform.

7

1. Place the robot’s end effector (pencil) at three different sample points on the floor using the jog keys

of the KUKA SmartPad. Mark one of the three sample points on the grid paper. We will need this

marked point later.

Make sure the points are not too close to each other. For each sample point, perform the following

measurements:

• Use the command getAngles() to read the joint angles of the robot at the sample point, and

store the returned value in a vector Qi, where i = 1, 2, 3 is the index of the sample point.

• On the SmartPAD, read the (x, y, z) coordinates of the tool tip in frame 0, and save them in a

vector Xi, i = 1, 2, 3. Make sure to choose “D pen” as your reference frame on the KUKA SmartPad. See the appendix for instructions on how to do this. Make sure to note these coordinates in

the same units as those used in your DH table (millimetres).

You have thus obtained these measurements: Qi

:= [θ

i

1

, θi

2

, θi

3

, θi

4

, θi

5

, θi

6

]

⊤ and Xi

:= [xi

, yi

, zi

]

⊤,

for i = 1, 2, 3.

• Using the vectors just collected, update variables X1, X2, X3, Q1, Q2, Q3 in the deltajoint.m

Matlab file (lines 6-11).

• Update variables X1, X2, X3 in the FrameTransformation.m Matlab file (lines 4-6). You will

use this function in the next two sections.

2. Duplicate your mykuka.m function and save it as mykuka search.m. This new function should work

like this:

myrobot = mykuka_search(delta)

where δ is a 2 × 1 vector containing the perturbations of the parameters a6, and d6. In other words,

the DH table in mykuka search.m should contain parameters a6 + δ(1), and d6 + δ(2).

3. Open and inspect the provided Matlab function deltajoint.m:

X_error = deltajoint(delta)

This function uses mykuka search.m and forward kuka.m to compute the sum of the errors between

the measured vectors Xi and their estimates computed using forward kinematics. Specifically,

H1=forward_kuka(Q1,kuka);

H2=forward_kuka(Q2,kuka);

H3=forward_kuka(Q3,kuka);

X_error=norm(H1(1:3,4)-X1)+norm(H2(1:3,4)-X2)+norm(H3(1:3,4)-X3);

You don’t have to make any change to this function, but make sure you understand its operation.

4. Use the Matlab command delta = fminunc(@deltajoint,[0 0]) to find the optimal vector delta

which minimizes the total joint variable error computed through the function deltajoint.m.

5. Redefine the robot structure using the updated DH parameters. You can do this by issuing the Matlab

command myrobot = mykuka search(delta), where delta is the vector of parameters you obtained

at the previous step.

6. Test your calibration. Command the robot to the HOME configuration. Solve an inverse kinematics

problem q = inverse kuka(H,myrobot), where the rotation matrix part of H is

R

0

6 =

0 0 1

0 − 1 0

1 0 0

8

and the translation part is o

9

6 = X1, where X1 is the sample point from step 1 that you have marked

on the grid paper. Then using setAngles(q,0.04), verify that the effector goes to the test point on

the grid paper, and that the pencil is pointing vertically downwards.

Note down the calibrated DH parameters a6 and d6 that you found. You will reuse them in Lab 4.

Show these results to your lab TA.

Base frame 0

Workspace frame w

Robot’s workspace

Figure 7: Base and Workspace frames of the Kuka robot.

4.4 Workspace frame versus Base frame

The base frame of the Kuka robot, displayed in Figure 7, is assigned by the manufacturer. The workspace of

the robot setup at the University of Toronto is the surface of a wood platform. This platform has a certain

thickness and is not perfectly horizontal. For this reason, we make use of a Workspace frame which we label

frame w, displayed in Figure 7, whose z axis is close to being vertical, but not perfectly so, and whose origin

is higher than the origin of the Base frame to reflect the thickness of the Workspace platform.

The Matlab function FrameTransformation.m provided to you utilizes the (Qi

, Xi) pairs you found in the

previous section to determine the homogeneous transformation matrix H0

w that converts points from the

coordinates of frame w to those of frame 0.

In Section 4.5 you will draw patterns on a sheet of paper taped on the robot’s Workspace. The specifications that will be given to you will be expressed in the coordinates of frame w, and you will use

FrameTransformation.m to convert them to frame 0. To get ready for drawing patterns, we now practice the conversion in question. To this end, we want to make the robot’s pencil reach the point p

w =

[600 100 10]⊤ (units are in millimetres). Before going forward, make sure to update variables X1, X2, X3

in the FrameTransformation.m Matlab file (lines 4-6), as detailed in Step 1 of Section 4.3.

1. Define the target point in Workspace coordinates: p workspace = [600; 100; 10].

2. Convert the point in base frame coordinates: p baseframe = FrameTransformation(p workspace).

3. Choose a desired orientation of the end effector with respect to the base frame (whereby the pencil

points vertically downward): R =[0 0 1;0 -1 0; 1 0 0 ].

9

4. Define the desired homogeneous transformation matrix of the end effector with respect to frame 0: H

= [R p baseframe; zeros(1,3) 1].

5. Use inverse kinematic to find the desired joint variables: q = inverse kuka(H,myrobot).

6. Command the robot to the target position: setangles(q,0.04).

4.5 Draw Patterns

Now that you have calibrated the robot, you are ready to draw patterns. The idea is to generate desired

end effector trajectories, use the inverse kuka.m function to translate them into desired joint trajectories,

and then commanding these joint trajectories to the robot. Throughout this section, we will want to keep

the pen vertical, pointing downward. This translates into requiring that R0

6 be given as

R

0

6 =

0 0 1

0 − 1 0

1 0 0

. (1)

In the development that follows, you will make o

0

6

follow paths corresponding to a number of patterns.

Meanwhile, R0

6 will be the constant constant matrix in (1).

1. Write a Matlab function mysegment.m performing these tasks. Generate a 3×100 matrix X workspace

containing 100 end effector positions in the coordinates of frame w describing a straight line segment

on the table parallel to the y

0 axis. Specifically, for i = 1, . . . , 100, X workspace(:,i) is a 3 × 1 vector

of the form [¯xi y¯i z¯i

]

⊤, where ¯xi = 620 mm, ¯yi ranges from − 100 mm to 100 mm and ¯zi = −1 mm

(this is to guarantee that the pencil makes contact with the paper).

Generate a 3 × 100 matrix X baseframe whose columns are the columns of X workspace converted to

the coordinates of frame 0 using the function FrameTransformation.m, as described in Section 4.4.

Next, for each column X baseframe(:,i) of X baseframe, form a matrix H using R0

6 as in (1), and o

0

6

equal to X baseframe(:,i). Compute the joint angles using inverse kuka.m and command them to

the robot using setangles in a loop.

Note: you may need to slightly adjust the ¯zi coordinates in the matrix X workspace to guarantee that

the pencil pushes down hard enough on the surface to create a visible line.

Show your results to the lab TA. Take a photo of the resulting pattern drawn by the

robot.

2. Write a Matlab function mycircle.m analogous to mysegment.m, but now such that the end effector

draws a circle of radius 50 mm centred at the point [620 0 − 1]⊤ in the workspace frame.

Show your results to the lab TA. Take a photo of the resulting pattern drawn by the

robot).

3. Invent a creative pattern, write a Matlab function for it, and make the robot draw it. Take a photo of

the result, and shoot a movie of the robot in action. For instance, try executing the pattern jug.xlsx

posted on Blackboard scaled by ten and centred about the origin of the frame marked on the grid

paper. Begin with the commands:

data=xlsread(’jug.xlsx’);

xdata=550 + 10*data(:,1);

ydata=10*data(:,2);

zdata=-ones(length(data),1);

Can you make something better?

10

5 Submission

Each student will submit on Quercus a preparation report in advance of the lab. No later than one week

after your lab session, submit on Quercus a zipped folder containing:

1. Your Matlab functions executing the steps in Section 4. The main file should be called Lab2.m. The

lab code should be thoroughly commented.

2. Photos of the three patterns drawn by the robot (segment, circle, and your pattern).

3. One movie of the robot drawing the creative pattern you came up with.

Appendix: How to read the coordinates of the pencil tip on the SmartPAD

1. Open the main menu ((A) in the figure below) on the SmartPAD, and select Display >Actual position

((B) in the figure below).

2. You should see a display showing the coordinates ( (C) in the figure below). The “Cartesian Position”

option should be selected (this can be toggled with “Axis Specific”, see (D)). Verify that the tool

selected is pencil and that the frame is $NULLFRAME (see (E)). Currently, on all machines this

is set to tool 11 and base 0. If the incorrect tool is selected, the coordinates will not be accurate.

11

3. The tool/base should be as shown in the image below. Note that the “Flange” option should be

selected.

4. If RSI ETHERNET.src is cancelled/rerun, the tool always defaults to tool[1]. After running RSI ETHERNET.src,

always change the tool back to the pencil tool.

12

Sale!

Uncategorized

# LAB2 : Forward and Inverse Kinematics for the KUKA Robotic Arm

$30.00

ROBOT MODELING AND CONTROL

ECE470S

LAB2 : Forward and Inverse Kinematics for the KUKA Robotic Arm

1 Purpose

The purpose of this lab is to adapt the results of lab 1 for implementation in a real robot. The objective is

to make the robot arm draw on paper.

2 Introduction

The KUKA robotic arm depicted in Figure 1 is an articulated manipulator with a three-degree-of-freedom

wrist and a gripper. Highlighted in Figure 1 are the axes of rotation of joints 1 to 6. A pencil, not displayed

Figure 1: The KUKA robotic arm in the HOME configuration