Chapter 3. Manipulator kinematics.

Page 14

CHAPTER 3. MANIPULATOR KINEMATICS.

3.1 Compute the kinematics of the planar arm from Example 3.3.

Frame assignments and link parameters:

X3

θ3

Y3
L2
Y2 X2 θ2

Y1 Y0
i αi-1 a i-1 di θi
L1 1 0 0 0 θ1
X1
2 0 L1 0 θ2
θ1
3 0 L2 0 θ3
X0

c1 − s1 0 0   c2 − s 2 0 L1  c3 − s3 0 L2 
s c1 0 0  1 s 0  s c3 0 0 
1T =
0  1 2T =
 2 c2 0 3T =
2  3
0 0 1 0 0 0 1 0 0 0 1 0
     
0 0 0 1 0 0 0 1 0 0 0 1
c123 − s123 0 L2 c12 + L1c1 
s c123 0 L2 s12 + L1 s1 
0
T = 0 1 2
T T T =  123
3 1 2 3
 0 0 1 0 
 
 0 0 0 1 

3.3 The arm with three degrees of freedom shown in Fig. 3.29 is like the one in
Example 3.3 except that joint 1’s axis is not parallel to the other two. Instead,
there is a wrist of 90 degrees in magnitude between axes 1 and 2. Derive link
parameters and the kinematic equations for WB T . Note that no l3 need be defined.

a. r./30.10.2003 2:55 /d:\my_files\2601050 robotics and teleoperation\year 2002-03\craig_book_complete\robot_book_3.doc

Albert Raneda Tampere University of Technology

 Chapter 3. Manipulator kinematics. Page 15

Frame assignments and link parameters:

X3

θ3

θ1
Y3
i αi-1 a i-1 di θi
L2
Y2 X2 θ2
1 0 0 0 θ1
Z01
2 90 L1 0 θ2
X01 3 0 L2 0 θ3
L1

 c1 − s1 0 0 c 2 − s 2 0 L1  c 3 − s3 0 L2 
s c1 
0 10 0 0 − 1 0  s c3 0 0 
0
T =  1 T =  2
T =  3
1
0 0 0 1
2
s2 c2 0 0
3
0 0 1 0
     
0 0 1 0 0 0 0 1 0 0 0 1
c1 c 23 − c1 s 23 s1 L2 c1 c 2 + L1 c1 
s c − s1 s 23 − c1 L2 s1 c 2 + L1 s1 
3T = 1T 2T 3T =
0 0 1 2  1 23

 s 23 c 23 0 L2 s 2 
 
 0 0 0 1 

3.4 The arm with three degrees of freedom shown in Fig. 3.30 has joints 1 and 2
perpendicular, and joints 2 and 3 parallel. As pictured, all joints are at their zero
location. Note that the positive sense of the joint angle is indicated. Assign link
frames {0} through {3} for this arm, that is, sketch the arm showing the
attachment of the frames. Then derive the transformation matrices.

Frame assignments and link parameters:

Y2 Y3

X2 X3

Z2 L3
Z3
θ1 i αi-1 a i-1 di θi
θ2 θ3
Z01
1 0 0 0 θ1
Y01 2 90 0 0 θ2
3 0 L3 0 θ3
X01

a. r./30.10.2003 2:55 /d:\my_files\2601050 robotics and teleoperation\year 2002-03\craig_book_complete\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 3. Manipulator kinematics. Page 16

 c1 − s1 0 0 c 2 − s2 0 0 c 3 − s3 0 L3 
s c1 0 0  0 0 −1 0  s c3 0 0 
1T =
0  1 2T =
1  3T =
2  3
0 0 1 0 s2 c2 0 0 0 0 1 0
     
0 0 0 1 0 0 0 1 0 0 0 1

3.6 Write a subroutine to compute the kinematics of the cylindrical arm in

Example 3.4. Use the procedure heading

Procedure KIN(VAR jointvar: vec3; VAR wrelb: frame);

Count a sine or cosine evaluation as costing 5 multiply times. Count additions as

costing 0.333 multiply times, and assignment statements as 0.2 multiply times.
Count a Square root computation as 4 multiply times. How many multiply times do
you need?

%kin_3_6
%
%Computes the kinematics of the cylindrical arm in Example 3.4
%
%kin_3_6(thetas,l2), where thetas = [theta1 d2 theta3]
%thetas in degrees and l2 in mm
%
%returns a 4x4 transforamtion matrix
%

function r = kin_3_6(thetas, l2);

t1 = thetas(1)*pi/180;
d2 = thetas(2);
t3 = thetas(3)*pi/180;

c1 = cos(t1);
c3 = cos(t3);
s1 = sin(t1);
s3 = sin(t3);
l = d2 - l2;
c_1 = -c1;
s_1 = -s1;

r = [ c1*c3 c_1*c3 s_1 l*s1

s1*c3 s_1*s3 c1 l*c_1
s3 c3 0 0
0 0 0 1];

a. r./30.10.2003 2:55 /d:\my_files\2601050 robotics and teleoperation\year 2002-03\craig_book_complete\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 3. Manipulator kinematics. Page 17

N cos,sin = 4 x 5 = 20
N assign = 7 x 2 = 1.4
N mult = 8 x 1 = 8
N add = 1 x 0.33 = 0.33

Multiple times = 29.73

3.7 Write a subroutine to compute the kinematics of the arm in Exercise 3.3. Use
the procedure heading

Procedure KIN(VAR theta: vec3; VAR vrelb: frame);

Count a sine or cosine evaluation as costing 5 multiply times. Count additions as

costing 0.333 multiply times, and assignment statements as 0.2 multipiy times.
Count a square root computation as 4 multiply times. How many multiply times
do you need?

%kin_3_7
%
%Computes the kinematics of the arm in Exercise 3.3
%
%kin_3_7(thetas,l)
%where thetas = [theta1 theta2 theta3] and l = [l1 l2]
%Thetas in degrees and l in mm
%
%returns a 4x4 transformation matrix
%

function r = kin_3_7(thetas, l);

t1 = thetas(1)*pi/180;
t2 = thetas(2)*pi/180;
t3 = thetas(3)*pi/180;
l1 = l(1);
l2 = l(2);

c1 = cos(t1);
s1 = sin(t1);
c_1 = c1*(-1);
s_1 = s1*(-1);
c2 = cos(t2);
s2 = sin(t2);
c23 = cos(t2 + t3);
s23 = sin(t2 + t3);
l2c2 = c2 * l2;

a. r./30.10.2003 2:55 /d:\my_files\2601050 robotics and teleoperation\year 2002-03\craig_book_complete\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 3. Manipulator kinematics. Page 18

r = [ c1*c23 c_1*s23 s1 l2c2*c1 + l1*c1

s1*c23 s_1*s23 c_1 l2c2*s1 + l1*s1
s23 c23 0 s2*l2
0 0 0 1];

N cos,sin = 6 x 5 = 30
N assign = 9 x 0.2 = 1.8
N mul = 12 x 1 = 12
N add = 4 x 0.33 = 1.33

Multiple times = 45.13

W
3.8 In Fig. 3.31 the location of the tool, T T , is not accurately known. Using force
control, the robot feels around with the tool tip until it inserts it into the socket
(or Goal) at location GS T . Once in this ’calibration’ configuration (in which {G}
B
and {T} are coincident), the position of the robot, W T , is determined by reading

the joint angle sensors and computing the kinematics. Assuming BST and GS T are
W
known, give the transform equation to compute the unknown tool frame, T T.

Calibration configuration : GS T = TS T
W
T T = WB T −1 ⋅ BS T ⋅GS T

3.9 For the two-link manipulator shown in Fig. 3.32a, the link transformation
matrices, 01T and 12T , were determined. Their product is 20T . The link frame

assignments are indicated in Fig. 3.32b. Note that frame {0} is coincident with
frame {1} when θ1 = 0. The length of the second link is l2. Find an expression for
the vector which locates the tip of the arm relative to the {0} frame.

 L2  c1 c 2 L2 + c1 L1 
 0  s c L + s L 
0
Ptip = 2T ⋅ Ptip = 2T ⋅   =  1 2 2
0 2 0 1 1

0  s 2 L2 
   
1  1 

a. r./30.10.2003 2:55 /d:\my_files\2601050 robotics and teleoperation\year 2002-03\craig_book_complete\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 3. Manipulator kinematics. Page 19

3.11 Figure 3.33 shows the schematic of a wrist with three intersecting axes, but
where the axis are not orthogonal. Assign link frames to this wrist (as if it was a
3-DOF manipulator), and give the link parameters.

Frame assignments and link parameters:

θ6

Z6

Y6 i αi-1 a i-1 di θi
4 0 0 0 θ4
5 φ 0 0 θ5
θ4 θ5
Y5
6 -φ 0 0 θ6
Z5

Z4 φ

Y4

3.16 Assign link frames to the RPR pianar robot shown in Fig. 3.36 and give the
linkage parameters.

Frame assignments and link parameters:

θ3
X2 X3

Z3

Z2 Y3
Y2
i αi-1 a i-1 di θi
L1 d2 1 0 0 0 θ1
X01 2 90 L1 d2 0
3 90 0 0 θ3
Y01

θ1

a. r./30.10.2003 2:55 /d:\my_files\2601050 robotics and teleoperation\year 2002-03\craig_book_complete\robot_book_3.doc

Albert Raneda Tampere University of Technology

 Chapter 3. Manipulator kinematics. Page 20

3.17 Show the attachment of link frames on the three-link robot shown in Fig.
3.37.

Frame assignments and link parameters:

X3

d3
Z3

Y3
X2
L2
i αi-1 a i-1 di θi
Y2
1 0 0 0 θ1
2 90 0 0 θ2+90
θ1 3 90 L2 d3 90
θ2
Z01
Y01

X01

3.18 Show the attachment of link frames on the three-link robot shown in Fig.
3.38.

Frame assignments and link parameters:

X3

Y3
i αi-1 a i-1 di θi
Z3
X2 1 0 0 0 θ1
L3 2 90 0 0 θ2+90
θ3 3 0 L3 0 θ3
Y2
Z2
θ1

θ2 Z01

Y01
X01

a. r./30.10.2003 2:55 /d:\my_files\2601050 robotics and teleoperation\year 2002-03\craig_book_complete\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 3. Manipulator kinematics. Page 21

3.19 Show the attachment of link frames on the three-link robot shown in Fig.
3.39.

Frame assignments and link parameters:

Y3
d3 X3

Z3
Z2
X2 L2
Y2
i αi-1 a i-1 di θi
θ1 1 0 0 0 θ1
2 0 0 d2 0
Z01 Y01 3 90 L2 d3 0
d2 X01

3.20 Show the attachment of link frames on the three-link robot shown in Fig.
3.40.

Frame assignments: θ2 θ3

Z1 Z2 Z3
Y1 Y2 Y3

X1 X2 X3

L2 L3
Z0 Y0
d1

X0

Link parameters:

i αi-1 a i-1 di θi
1 0 0 d1 0
2 0 L2 0 θ2
3 0 L3 0 θ3

a. r./30.10.2003 2:55 /d:\my_files\2601050 robotics and teleoperation\year 2002-03\craig_book_complete\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 3. Manipulator kinematics. Page 22

3.21 Show the attachment of link frames on the three-link robot shown in Fig.
3.41.

Frame assignments and link parameters:

Z3

X0 Y3

Y0
X3
d3 i αi-1 a i-1 di θi
d1 X1 Z0
d2 1 0 0 d1 0
Y1 2 90 0 d2 90
Z2 3 90 0 d3 0
X2
Z1 Y2

Programming Exercises.

1. Write a subroutine to compute the kinematics of the planar 3R robot in

Example 3.3. That is, a routine with the joint angles' values as input, and a frame
(the wrist frame relative to the base frame) as output. Use the procedure
heading

Procedure KIN(VAR theta: vec3; VAR wrelb: frame);

where ’wrelb’ is the wrist frame relative to the base frame, WB T . The type ’frame’

consists of a 2 x 2 rotation matrix and a 2 x 1 position vector. If desired, you may

represent the frame with a 3 x 3 homogeneous transform in which the third row
is [ 0 0 1 ]. (The manipulator data are 11 = 12 = 0.5 meters.)

a. r./30.10.2003 2:55 /d:\my_files\2601050 robotics and teleoperation\year 2002-03\craig_book_complete\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 3. Manipulator kinematics. Page 23

% KIN_3_1(thetas)
% %
% Changes representation from internal form to user form.
%
% KIN_3_1(thetas), where thetas = [theta1 theta2 theta3] and
% angles are in degrees.
%
% returns a (3x3) transformation matrix: the wrist frame relative
% to the base frame

function wrelb = kin_3_1(thetas);

t1 = thetas(1)*pi/180;
t2 = thetas(2)*pi/180;
t3 = thetas(3)*pi/180;
l1 = 0.5;
l2 = 0.5;

wrelb = [ cos(t1+t2+t3) -sin(t1+t2+t3) l1*cos(t1)+l2*cos(t1+t2)

sin(t1+t2+t3) cos(t1+t2+t3) l1*sin(t1)+l2*sin(t1+t2)
0 0 1];

2. Write a routine which calculates where the tool is relative to the station frame.
The input to the routine is a vector of joint angles:

Procedure WHERE(VAR theta: vec3; VAR trels: frame);

Obviously, WHERE must make use of descriptions of the tool frame and the
robot base frame in order to compute the location of the tool relative to the
station. The value of W
T T and BST should be stored in global memory (or ,as a

second choice, you may pass them as arguments in WHERE).

% WHERE
%
% Calculates where the tool is relative to the station frame
%
% WHERE(thetas, trelw, srelb), where thetas = [theta1 theta2 theta3]
% and trelw and srelb are transformation matrices:
% tool frame relative to wrist frame and
% station frame relative to base frame respectively.
%
% Input angles are in degrees.
%
% returns a (3x3) transformation matrix: the tool frame relative
% to the station frame in User Form

function trels = where(thetas, trelw, srelb);

wrelb = kin_3_1(thetas);

trels = itou(inv(utoi(srelb))*wrelb*utoi(trelw));

a. r./30.10.2003 2:55 /d:\my_files\2601050 robotics and teleoperation\year 2002-03\craig_book_complete\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 3. Manipulator kinematics. Page 24

3. A tool frame and a station frame are defined by the user for a certain task
as below:

W
T T = [x y 0] = [ 0.1 0.2 30.0],
B
S T = [x y θ] = [ -0.1 0.3 0.0].

Calculate the position and orientation of the tool relative to the station frame for
the following three configurations (in units of degrees) of the arm:

[θ1 θ2 θ3 ] = [ 0.0 90.0 -90.0],

[θ1 θ2 θ3 ]= [ -23.6 - 30.3 48.0],
[θ1 θ2 θ3] = [130.0 40.0 12.0].

» trelw = [0.1 0.2 30]; srelb = [-0.1 0.3 0];

» thetas = [0 90 -90];
» trels = where(thetas, trelw, srelb)

trels =

0.7000 0.4000 30.0000

» thetas = [-23.6 -30.3 48.0];

» trels = where(thetas, trelw, srelb)

trels =

0.9728 -0.7155 24.1000

» thetas = [130.0 40.0 12.0];

» trels = where(thetas, trelw, srelb)

trels =

-0.8068 -0.0335 -148.0000

a. r./30.10.2003 2:55 /d:\my_files\2601050 robotics and teleoperation\year 2002-03\craig_book_complete\robot_book_3.doc

Albert Raneda Tampere University of Technology