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:
Z
2
13
7
6
4.2 Derive the inverse kinematics of the three-link manipulator of Chapter 3,
Exercise 3.
c1c 23
s c
B
B S W −1 0
 1 23
=
=
=
T
T
T
T
T
W
S T T
3
 s 23

 0
c1c 23
s c
B
0
 1 23
T
=
T
=
W
3
 s 23

 0
− c1 s 23
− s1 s 23
c23
0
s1
− c1
0
0
− c1 s 23
− s1 s 23
c 23
0
s1
− c1
0
0
c1c2 L2 + c1 L1 
s1c 2 L2 + s1 L 

s 2 L2

1

c1c 2 L2 + c1 L1   r11
s1c2 L2 + s1 L  r21
=
 r31
s 2 L2
 
1
 0
r12
r22
r32
0
r13
r23
r33
0
px 
p y 
pz 

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 

 − r23 
θ 1 = tan −1 
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
L2
If c1 is not 0 then c 2 =
1
L2
If c1 = 0 then c 2 =

 px

− L1 

 c1
 py


− L1 
 s1

Using pz = s2L2 from element (3,4) we can solve:
 p z L2 

c
 2 
θ 2 = tan −1 
Finally for θ3,
Equate elements (3,1): s23 = r31
Equate elements (3,2): c23 = r32
 r31 
 = θ 2 + θ 3 → θ 3 = θ 23 − θ 2
 r32 
θ 23 = tan −1 
4.3 Sketch the fingertip workspace of the 3-DOF manipulator of Chapter 3,
Example 3.4.
Circle of radius d2.
Z
d2
X
Y
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.
− s1
c1
0
0
 c1
s
0
 1
T
=
1
0

0
0
0
1
0
− c1 s 3
− s1 s 3
c3
0
 c1 c 3
s c
0
 1 3
3T =
 s3

 0
0
0 
0

1
1
0
1

T
=
2
0

0
0 0
0 
0 − 1 − d 2 
1 0
0 

0 0
1 
s1
− c1
1
0
( d 2 + L2 )s1   r11
− ( d 2 + L2 )c1  r21
=
 r31
0
 
1
 0
c 3
s
2
 3
T
=
3
0

0
r12
r22
r32
0
r13
r23
1
0
− s3
c3
0
0
0 0
0 0 
1 L2 

0 1
px 
p y 
0

1
Equate elements (1,3): s1 = r13
Equate elements (2,3): -c1 = r23
 r13 

 − r23 
θ 1 = tan −1 
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
d2 =
X2
p x2 + p y2 − L2
 px
 − py

θ 1 = tan −1 
X0




X1
L2
θ1
Z3
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
1
2
3
4
αi-1
a i-1
0
a1
0
a3
0
0
α2
0
θi
di
0
0
d3
0
θ1
θ2+90
θ3-90
θ4
α2 = 45, a1 = 1, d3 = 2 , a3 = 2
 c1
s
0
 1
=
T
1
0

0
− s1
c1
0
0
 c3

2
s3
2
 2
3T =

2
s3 2
 0

0
0
1
0
− s3
2
c3
2
2
c3
2
0
c 2
s
1
 2
=
T
2
0

0
0
0 
0

1
0
−
2
2
2
2
0
0

− 1


1
1 
− s2
c2
0
0
0 a1 
0 0 
1 0

0 1
c 4
s
3
 4
4T =
0

0
− s4
c4
0
0
0 a3 
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
T = 01T 12T 23T 34T
0
4
T 04T = 10T 01T 12T 23T 34T = 41T
1
0
 c1
− s
1
 1
0T =
 0

 0
p z = a3 s3
s1
c1
0
0
0
0
1
0
0
0 
0

1

...

...
1
4T = 

...

 0

2
+ s 2 + a1 
2

2
a3 c3 s 2 + a 3 s 3 c 2
− c2 

2

2

a3 s 3
2

1

... ... a3 c3 c 2 − a 3 s 3 s 2
... ...
... ...
0
0
2
⇒ 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
s3 +
2
2
2
2
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
f 3 = a3 sα 2 s 3 − d 4 sα 3 sα 2 c3 + d 4 cα 3 cα 2 + d 3 cα 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
ans_t3_2
ans_t3_1
ans_t3_2
=
=
=
=
t - ans_t2_1 - ans_t1_1;
t - ans_t2_2 - ans_t1_2;
checkang(ans_t3_1);
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
119.3181
-18.9582
108.6629
-85.2952
108.6629
-85.2952
current =
-85.3599
current =
66.6323
current =
66.6323
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