Chapter 4. Inverse Manipulator kinematics.

Page 25

CHAPTER 4. INVERSE MANIPULATOR KINEMATICS.

4.1 Sketch the fingertip workspace of the three-link manipulator of chapter 3,

Exercise 3 for the case 11 = 15.0, l2 = 10.0, and l3 = 3.0.

The workspace is a rotation of a ring about z:

2 13 7 6

4.2 Derive the inverse kinematics of the three-link manipulator of Chapter 3,

Exercise 3.
c1c 23 − c1 s 23 s1 c1c2 L2 + c1 L1 
s c − s1 s 23 − c1 s1c 2 L2 + s1 L 
B B S W −1 0  1 23
W T = T T
S T TT = T =
3  s 23 c 23 0 s 2 L2 
 
 0 0 0 1 

c1c 23 − c1 s 23 s1 c1c 2 L2 + c1 L1   r11 r12 r13 px 

s c − s1 s 23 − c1 s1c2 L2 + s1 L  r21 r22 r23 p y 
B
W T = 0
T =  1 23 =
3  s 23 c23 0 s 2 L2  r31 r32 r33 pz 
   
 0 0 0 1  0 0 0 1 

a. r./6.11.2003 1:30 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 4. Inverse Manipulator kinematics. Page 26

Equate elements (1,3): s1 = r13

Equate elements (2,3): -c1 = r23
 r13 
θ 1 = tan −1  
 − r23 
If r13 = 0 and r23 = 0, the goal is unattainable.

Equate elements (1,4): px = c1(c2L2+L1)

Equate elements (2,4): py = s1(c2L2+L1)

1  px 
If c1 is not 0 then c 2 =  − L1 
L2  c1 

1  py 
If c1 = 0 then c 2 =  − L1 
L2  s1 
Using pz = s2L2 from element (3,4) we can solve:
 p z L2 
θ 2 = tan −1  
 2 c

Finally for θ3,

Equate elements (3,1): s23 = r31
Equate elements (3,2): c23 = r32
 r31 
θ 23 = tan −1   = θ 2 + θ 3 → θ 3 = θ 23 − θ 2
 r32 

4.3 Sketch the fingertip workspace of the 3-DOF manipulator of Chapter 3,

Example 3.4.

Circle of radius d2.

d2

Y
X

a. r./6.11.2003 1:30 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 4. Inverse Manipulator kinematics. Page 27

4.4 Derive the inverse kinematics of the 3-DOF manipulator of Chapter 3,

Example 3.4.

 c1 − s1 0 0 1 0 0 0  c 3 − s3 0 0
s c1 0 0  0 0 − 1 − d 2  s c3 0 0 
0
T =  1 1
T =  2
T =  3
1
0 0 1 0
2
0 1 0 0 
3
0 0 1 L2 
     
0 0 0 1 0 0 0 1  0 0 0 1

 c1 c 3 − c1 s 3 s1 ( d 2 + L2 )s1   r11 r12 r13 px 

s c − s1 s 3 − c1 − ( d 2 + L2 )c1  r21 r22 r23 p y 
3T =
0  1 3 =
 s3 c3 1 0  r31 r32 1 0
   
 0 0 0 1  0 0 0 1

Equate elements (1,3): s1 = r13

Equate elements (2,3): -c1 = r23
 r13 
θ 1 = tan −1  
 − r23 
If r13 = 0 and r23 = 0, the goal is unattainable.
r 
Same for θ3 with elements r31 and r32: θ 3 = tan −1  31 
 r32 
Knowing θ1 we can calculate d2 by equating (1,4) or (2,4)

We can use also a geometric approach:

X3
X2
d2 = p x2 + p y2 − L2 X0
L2
θ1
 px  X1
θ 1 = tan −1   Z3
 − py 
 
d2
px
Z2
Y0
-py

Y1

a. r./6.11.2003 1:30 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 4. Inverse Manipulator kinematics. Page 28

4.6 Describe a simple algorithm for choosing the nearest solution from a set of
possible solutions.

if (solution1 – current) >(solution2 – current) then

far = solution1
near = solution2
else
far = solution2
near = solution1
endif

4.9 Figure 4.13 shows a two-link planar arm with rotary joints. For this arm, the
second link is half as long as the first, that is: L1 = 2L2 . The joint range limits in
degrees are:
0 < θ1 < 180,
-90 < θ2 < 180.

Sketch the approximate reachable workspace (an area) of the tip of link 2.

a. r./6.11.2003 1:30 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 4. Inverse Manipulator kinematics. Page 29

4.16 A 4R manipulator is shown schematically in Fig. 4.15. The nonzero link

parameters are a1 = 1, α2 = 45, d3 = 2 , a 3 = 2 , and the mechanism is pictured

in the configuration corresponding to θ = [0 90 –90 0]T. Each joint has limits of

±180. Find all values of θ3 such that 0 P4 ORG = [1.1 1.5 1.707]T.

Link parameters and transformation matrices:

i αi-1 a i-1 di θi
1 0 0 0 θ1
2 0 a1 0 θ2+90
3 α2 0 d3 θ3-90
4 0 a3 0 θ4

α2 = 45, a1 = 1, d3 = 2 , a3 = 2

 c1 − s1 0 0 c 2 − s2 0 a1 
s c1 0 0  s c2 0 0 
0
T =  1 1
T =  2
1
0 0 1 0
2
0 0 1 0
   
0 0 0 1 0 0 0 1

 c3 − s3 0 0
  c 4 − s4 0 a3 
2 2 2
s3 c3 − − 1 s c4 0 0 
 2   4
3T = 4T =
2 2 2 3

 2 2 2  0 0 1 0
s3 2 c3 1  
 0 2 2 0 0 0 1
 0 0 1 

a. r./6.11.2003 1:30 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 4. Inverse Manipulator kinematics. Page 30

0
4 T = 01T 12T 23T 34T
1
0 T 04T = 10T 01T 12T 23T 34T = 41T
 2 
... ... ... a3 c3 c 2 − a 3 s 3 s 2 + s 2 + a1 
 c1 s1 0 0  2 
− s 0  ... − c2 
2
 1 c1 0 ... ... a3 c3 s 2 + a 3 s 3 c 2
0T = 4T = 
1 1
2 
 0 0 1 0  
  ... ... ... a3 s 3
2

 0 0 0 1
 2 
 0 0 0 1 

2
p z = a3 s3 ⇒ s 3 = 0.707 → θ 3 = 45 or 135
2

Another way is to use Pieper's solution:

The point of intersection of the three last axes is 0 P4 ORG .

0
P4 ORG = 01T 12T 23T 3P4 ORG

U sin g z = (k 1 s 2 − k 2 c 2 )sα 1 + k 4
and knowing that sα 1 = 0 , we have z = k 4

Then , z = f 3 + d 2 = f 3
z = 1.707 = f 3 = a 3 sα 2 s 3 − d 4 sα 3 sα 2 c3 + d 4 cα 3 cα 2 + d 3 cα 2 = s 3 + 1 ⇒
⇒ s 3 = 0.707 → θ 3 = 45 or 135

4.17 A 4R manipulator is shown schematically in Fig. 4.16. The nonzero link

parameters are α1 = -90, d2 =1, α2 =45, d3 = 1 and a3 = 1, and the mechanism is
pictured in the configuration corresponding to θ = [0 0 90 0]T. Each joint has
limits of ±180. Find all values of θ3 such that 0 P4 ORG = [0.0 1.0 1.414]T.

Using Pieper's solution:

The point of intersection of the three las axes is 0 P4 ORG .

a. r./6.11.2003 1:30 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 4. Inverse Manipulator kinematics. Page 31

 0 
0
P4 ORG =  1 
1.414 
a1 = 0 → k 3 = r
r = 0 2 + 12 + 1.414 2 = 3

2 2
f 3 = a3 sα 2 s 3 − d 4 sα 3 sα 2 c3 + d 4 cα 3 cα 2 + d 3 cα 2 = s3 +
2 2
k 3 = 3 = a3 + d 4 + d 3 + a 2 + 2d 4 d 3 cα 3 + 2a 2 a 3 c 3 + 2a 2 d 4 cα 33 s 3 + a12 + d 22 + 2d 2 f 3
2 2 2 2

= 3 + 2 s 3 + 2 ⇒ s 3 = −1 → θ 3 = −90

Programming Exercise.

1. Write a subroutine to calculate the inverse kinematics for the three-link

manipulator (see Section 4.4). The routine ahould pasa arguments as shown
below:

Procedure INVKIN(VAR wrelb: frame; VAR current,near,far: vec3;

VAR sol:boolean);

where "wrelb," an input, is the wtist frame specified relative to the base frame;
"current," an input, is the current position of the robot (given as a vector of joint
angles); "near" is the nearest solution; "far" is the second solution; and "sol" is
a flag which indicates whether solutions were found or not. (sol = FALSE if no
solutions were found). The link lengths (meters) are 11 = 12 = 0.5.

The joint ranges of motion are: 170 < θi < 170. Test your routine by calling it
back-to-back with KIN to test whether they are indeed inverses of one another.

% INVKIN
%
%
% Calculates inverse kineamtics for the 3-link manipulator.
%

a. r./6.11.2003 1:30 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 4. Inverse Manipulator kinematics. Page 32

% INVKIN(wrelb, current), where wrelb=[x y angle] and

% current = [theta1 theta2 theta3] with angle and thetas in degrees
%
% returns 2 vectors (1x3) with the near solution and the far
% far solution, and a flag to indicate if a solution was found

function [near, far, flag] = invkin(wrelb, current);

l1 = 0.5;
l2 = 0.5;

x = wrelb(1);
y = wrelb(2);
t = wrelb(3);

c2 = (x*x + y*y - l1*l1 -l2*l2)/(2*l1*l2);

if (c2>1 | c2<-1),
flag = 0;
near = current;
far = current;
else
s2_1 = sqrt(1-c2*c2);
s2_2 = -sqrt(1-c2*c2);
ans_t2_1 = atan2(s2_1,c2)*180/pi;
ans_t2_2 = atan2(s2_2,c2)*180/pi;

if (x==0 & y==0),

flag = 0.5;
ans_t1_1 = 0;
ans_t1_2 = 0;
else
flag = 1;
k1 = l1 + l2*c2;
k2_1 = l2*s2_1;
k2_2 = l2*s2_2;
ans_t1_1 = (atan2(y,x)-atan2(k2_1,k1))*180/pi;
ans_t1_2 = (atan2(y,x)-atan2(k2_2,k1))*180/pi;
ans_t1_1 = checkang(ans_t1_1);
ans_t1_2 = checkang(ans_t1_2);
%checkang checks that the joints are within their ranges

end

ans_t3_1 = t - ans_t2_1 - ans_t1_1;

ans_t3_2 = t - ans_t2_2 - ans_t1_2;
ans_t3_1 = checkang(ans_t3_1);
ans_t3_2 = checkang(ans_t3_2);

ans_1 = [ans_t1_1 ans_t2_1 ans_t3_1];

ans_2 = [ans_t1_2 ans_t2_2 ans_t3_2];

if (norm(ans_1 - current) > norm(ans_2 - current)),

far = ans_1;
near = ans_2;
else
far = ans_2;
near = ans_1;

a. r./6.11.2003 1:30 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 4. Inverse Manipulator kinematics. Page 33

end

end

if nargout == 0,
flag
near
end

2. A tool is attached to link 3 of the manipulator. This tool is described by ,wT,

the tool frame relative to the wrist frame. Also, a user has described his work
area, the station frame relative to the hase of the robot, as BT. Write the
subroutine

Procedure SOLVE(VAR trels: frame; VAR current,near,far: vec3;

VAR sol:boolean);

where "trels" is the {T} frame specified relative to the {S} frame. Other
parameters are exactly as in the INVKIN subroutine. The definitions of {T} and
{S} should be globally defined variables or constants. SOLVE should use calls to
TMULT, TINVERT, and INVKIN.

% SOLVE
%
%
% Calculates inverse kineamtics for the 3-link manipulator.
% SOLVE calculates wrelb and solves for joints 1, 2 and 3
%
% SOLVE(goal, tool, station, current), where goal, tool and
% station are given in user form.
%
% returns 2 vectors (1x3) with the near solution and the far
% far solution, and a flag to indicate if a solution was found

function [near, far, flag] = solve(goal, tool, station, current);

trels = utoi(goal);
srelb = utoi(station);
trelw = utoi(tool);

a. r./6.11.2003 1:30 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 4. Inverse Manipulator kinematics. Page 34

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

[near, far, flag]= invkin(wrelb, current);

3. Write a main program which accepts a goal frame specified in terms of x, y,

and θ. This goal specification is {T) relative to {S}, which is the way the user
wants to specify goals.

The robot is using the same tool in the same working area as in Programming
Exercise (Part 2), so {T} and {S} are defined.

Calculate the joint angles for each of the three goal frames given below.
Assume that the robot will start with all angles equal to 0.0 and move to these
three goals in sequence. The program should find the neareat solution with
respect to the previous goal point.
[x1 y1 θ1 ] = [0.0 0.0 -90.0],
[x2 y2 θ2 ] = [0.6 -0.3 45.0],
[x3 y3 θ3 ] = [-0.4 0.3 120.0],
[x4 y4 θ4 ] = [0.8 1.4 30.0].

You should call SOLVE and WHERE back to back to make sure they are truly
inverse functions.

% PROG_4_3
%
% Prints the joint values for every goal frame for
% the 3-link manipulator
%
% PROG_4_3(goal_frame) where goal frame is matrix, where each
% row is a goal especifation of the form [x y angle]. Angle in
degrees
%

function joints = prog_4_3(goal_frame);

n = size(goal_frame);

tool = [0.1 0.2 30];

a. r./6.11.2003 1:30 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology

Chapter 4. Inverse Manipulator kinematics. Page 35

station = [-0.1 0.3 0];

current = [0 0 0];

for i = 1:n(1),
goal = goal_frame(i,1:3);
[near far flag] = solve(goal, tool, station, current);
current = near

if flag == 0,
i = n;
end

end

Using prog_4_3 with the goal points:

» goal = [0 0 -90 ; 0.6 -0.3 45 ; -0.4 0.3 120 ; 0.8 1.4 30.0 ];
» prog_4_3(goal)

current =

57.0088 115.2643 67.7269

current =

-85.3599 119.3181 -18.9582

current =

66.6323 108.6629 -85.2952

current =

66.6323 108.6629 -85.2952

a. r./6.11.2003 1:30 /d:\my_files\2601050 robotics and teleoperation\year 2003-04\craig_book\robot_book_3.doc

Albert Raneda Tampere University of Technology