Forward and inverse kinematics of the

teach-robot
TM

Bart De Vylder
bartdv@arti.vub.ac.be
Artificial Intelligence Lab - Vrije Universiteit Brussel
Pleinlaan 2, 1050 Brussels, Belgium
Abstract
The goal of this paper is to provide a practical reference about the
forward and inverse kinematics of the teach-robot 1 . Not only the relation
between the end-effector position and the joint angles is derived, which
corresponds to the well described domain of robot kinematics, but also
the non-linear relation between these angles and the actual values with
which the motors of the robot have to be controlled.

Introduction and conventions

First we give a short description of the teach-robot. Figure 1 names the different
motors and parts. The robot has six DC servo motors. Motor 1 opens/closes
the gripper, motor 2 turns the wrist. Motor 3, 4 and 5 are linear motors which
drive respectively the wrist, the arm and the shoulder. Motor 6 rotates the
body (and therefore the whole robot) around a vertical axis. On a low level
the rotation of a motor is controlled by pulses; one pulse makes a motor turn
over a fixed angle in a desired direction. To allow higher level control, a value is
associated with the position of each motor-driven component of the robot. The
value of a component position represents the number of pulses the corresponding
motor should receive in order to drive that component to that position, starting
from a fixed reference position. For each component this reference position is
physically detectable with a touch sensor and has by definition value 0 (340 for
the body, driven by motor 6). The robot can now be controlled by sending the
desired component positions to the corresponding motors. The value one can
send to a motor lies between 0 and a motor dependent maximum:
1 teach-robot

is a trademark of Microelectronic Kalms Gmbh.

Figure 1: Naming of the motors and the different parts of the robot
Motor
1
2
3
4
5
6

Maximum value
70
130
420
420
350
700

In what follows we name the value we send to motor i, mi . We define the

end-point of the robot to be the point in space central between the two round,
black taps on the gripper when the gripper would be closed. Section 2 treats the
problem of deriving the motor values from the coordinates of the end-point and
section 3 vice versa. We introduce a right-handed cartesian coordinate frame
fixed with respect to the bottom plate of the robot (see figure 2). The Z-axis
coincides with the vertical rotation axis of the body. We choose the origin of this
axis on the same height as the link between the body and the shoulder of the
robot. The Z-axis points upward. The X-axis has the direction of the longest
sides of the bottom plate and points to the front of the robot (in reset position).
The Y -axis has the direction of the shortest sides of the bottom plate, its sense
follows from these of the X- and Z-axis.
It follows from the definition of the end-point that it is independent of m1 .
In addition we assume that the end-point lays on the rotation axis of the wrist
so that it is also independent of m2 (the wrist can be adjusted manually to fulfill
this requirement). Furthermore instead of working with cartesian coordinates
(x, y, z), we will adopt cylindrical coordinates (r, , z). One can easily switch
2

Figure 2: Cartesian and cylindrical coordinate frame fixed relative to the bottom
plate of the robot. The XY -plane contains the link between the body and the
shoulder of the robot.

between the coordinate frames using the following conversion rules:

p
x = r cos
r = x2 + y 2
y = r sin
z=z

= atan2(y, x)
z=z

The function atan2(y, x) computes arctan(y/x) but utilizes the sign of each
argument to determine which quadrant the resulting angle belongs to; this allows
the correct determination of an angle in a range of 2 (see [2] p. 31). With the
use of cylindrical coordinates the relation


m3
r
m4

m5
z
m6
can be rewritten as

 
m3
m4 r
z
m5

m6

which simplifies the calculations as we shall see in the following sections.

Inverse kinematics

In this section we describe a method which allows us to obtain the motor values (m3 , m4 , m5 , m6 ) from the coordinates (r, , z). The calculation of m6 is
straightforward and explained in section 2.3. How to compute m3 , m4 and m5
is explained in the following sections.

2.1

Derivation of the joint angles

In this part we tackle the classical inverse kinematical problem of computing the
robot-arm-joint angles as a function of the coordinates of his end-point. The
treatment of a similar problem can be found in [2] p. 67-70. Figure 3 contains
the definition of several angles concerning the robot arm in the RZ-plane. We
adopt the convention that an angle is positive if it is oriented counter-clockwise.
Angles are measured in radians. Point P3 corresponds to the end-point of the
robotarm and has coordinates (r, z) in the RZ-plane. In the figure can be seen
that motor 3 determines 3 , motor 5 determines 5 , and 4 depends both on
m4 and m5 . If we direct the robot arm to a point in the RZ-plane with the
three motors 3, 4 and 5, we have one degree of freedom left. In what follows we
assume that 3 is known. How 3 will be determined is explained in section 2.4.
In triangle P3 P4 P5 we apply the cosine-rule and become
2

a4 = a3 2 + a4 2 2 a3 a4 cos( 3 )
4

P4
1
0
0
1

a4

3
a3

11
00
P
5

0P3 (r, z)
1

a4

a3
a4
a5
d

a5

a5

17.8
20.0
20.0
3.8

cm
cm
cm
cm

11
00

P6

Figure 3: Definition of different angles and lengths used for the inverse kinematical problem. The figure is only meant to define the different variables and
constants, the scale is not correct.

or
a4 =

a3 2 + a4 2 + 2 a3 a4 cos 3 .

In the same triangle we obtain (by applying the sine-rule)

sin 3
sin
=
a3
a4
or
= arcsin


a3
sin

3 .
a4

The function arcsin can only return a value between /2 and /2. The mechanics of the arm however prevent to lie outside this range. Note also that
will have the same sign as 3 . Subsequently we focus on triangle P5 P3 P6 and
apply the cosine-rule:
2

a5 = a4 + a5 2 + 2 a4 a5 cos 4
or

= arccos

a5 a5 2 a4
2a4 a5

The mechanics of the robotarm force 4 to be negative what is the reason

for choosing the minus sign in the last formula. The distance a5 is equal to
p
r 2 + z 2 where r = r d. Given the angles 4 and we obtain
4 = 4 .

Applying the cosine-rule in triangle P5 P3 P6

2

a4 = a5 2 + a5 2 a5 2 a5 cos
yields
2

= arccos

a4 a5 2 a5
2a5 a5

Again the construction of the robot assures to lie within the range 0 to of
the function arccos. The angle can be calculated as follows
= atan2(z, r )
which finally results in
5 = + .
In this section we derived the angles 4 and 5 (3 was given). Now we have
to relate those angles to the appropriate motor values m3 , m4 and m5 . As an
intermediate step we calculate the lengths the linear motors 3, 4 and 5 have to
take up in order to position the corresponding components at the correct angles.

d3

f4
M4
11
00
00
11
4
N5

0
1
0
1 1
0
0 P5
1

f3

11
00
P4

3
3
a3

11
00
P3
00
11

a4

a5

f5

N6

d5

1
0

3
e3

d4

M6

11
00
N4
00
11

1
0

e5

11 00
00
11
P

M5

f4
a3
a4
a5
e3
e5
f3
f4
f4
f5
3
3
5

17.8
20.0
20.0
19.9
20.4
4.0
4.0
4.0
4.0
0.209
1.518
0.196

cm
cm
cm
cm
cm
cm
cm
cm
cm
rad
rad
rad

Figure 4: Definition of new variables and constants used for deriving the lengths
d3 , d4 and d4 . (M6 , N6 ), (M5 , N5 ) and (M4 , N4 ) are the junctures of linear
motors 5, 4 and 3 respectively. M6 is fixed with respect to the body (bottom
horizontal line) and M4 is fixed with respect to the arm. Is is safest to remeasure
the constants, especially 3 which depends on the way the gripper is fastened.

2.2

Derivation of the length of motors 3, 4 and 5

The previous section described how to calculate the angles 4 and 5 . There is
however no linear relation between these angles and the motor values m4 and
m5 , nor between 3 and m3 . We introduce a new bulk of definitions in figure 4.
We need to compute d3 , d4 and d5 from the angles 3 , 4 and 5 . In the previous
section we assumed 3 to be known in advance. In fact we will choose the value
m3 , derive d3 from it and then compute 3 . Anyhow the inverted calculation is
given for completeness. First observe that
3 = 3 3 3 .
Next we apply the cosine-rule in triangle M4 N4 P4
d3 2 = e3 2 + f3 2 2 e3 f3 cos 3
or

q
d3 = e3 2 + f3 2 2 e3 f3 cos 3 .

To obtain d5 we apply the cosine-rule in triangle M6 N6 P6

d5 2 = e5 2 + f5 2 2 e5 f5 cos( 5 5 )
or
d5 =

e5 2 + f5 2 + 2 e5 f5 cos(5 + 5 ).

The length d4 depends both on 5 and 4 . In rectangle N5 P5 P6 M5 we obtain

d4 2 = (f4 sin 4 f4 sin 5 )2 + (a5 + f4 cos 4 f4 cos 5 )2
which can be rewritten as
q
d4 = (f4 sin 4 + f4 sin 5 )2 + (a5 f4 cos 4 + f4 cos 5 )2

because 4 = + 4 and 5 = 5 .
Once d3 , d4 and d5 are known, we have to use the relationship between the
value sent to a linear motor and his length in order to determine m3 , m4 and
m5 . This is explained in the next section.

2.3

Motor properties

In this section we describe the motor behaviour of motors 3, 4, 5 and 6. Motors

1 and 2 have no influence on the end-point of the robot and are not considered.
Motors 3, 4 and 5 are linear. The relationship between mi and di (i
{3, 4, 5}) is thus given by
d i vi
mi =
.
ui
The value ui is the same for the three motors and equals approximately 0.0124 cm.
The value vi is the length of motor i in position 0:
8

Figure 5: 2-D workspace of the robotarm for m3 consequently equal to 0, 160

and 420. For each value of m3 the border of the workspace was drawn by
attaching a pen to the end-point of the robot. The unit on the R and Z-axis is
cm.
v3
v4
v5

17.2 cm
17.2 cm
17.7 cm

Note that it is safest to remeasure these values when dealing with a new robot.
There is also a linear relation between (the angle over which the body is
turned) and m6 . The slope (change in motor value per change in angle) equals
approximately 129.87. When m6 = 340 then = 0 so we obtain
m6 = 129.87 + 340.

2.4

Choice for the wrist position

Like already mentioned we have one degree of freedom left if we want to guide
the robotarm to a certain point in space (the four-dimensional space determined
by m3 , m4 , m5 , m6 is mapped onto the three-dimensional space of possible
end-point positions). Normally a specification of the desired orientation of the
end-effector is used to solve the undeterminacy. We do not follow this approach
because the set of possible orientations that the end-effector can take on with
a fixed end-point is rather small and depends highly on the position of the
end-point, as is pointed out in figure 5. Therefore, based on the coordinates
(r, z) of the goal position in the RZ-plane, we choose a value m3 with which we
know the goal position can be reached. The method we present here is not very
9

complicated but seemed to be sufficient for our purposes. Undoubtly, certain

types of task will require a more advanced solution.
We choose an appropriate line in the RZ-plane
ar + bz + c = 0,
a border width and values m3 min and m3 max . Then we compute


1
(ar + bz + c)
m3 = f
a2 + b2
with
f (x) =

m3max

m3min m3max
x
2

m3min +m3max
2

m3min

if x < ,
if x < ,
if x.

The used values are

a
b
c

m3 min
m3max

7
3
280
2
160
420

cm
cm

How to obtain 3 from m3 is described in section 3.

2.5

Summary

The described technique in this section provides us with a method to calculate

the motor values (m3 , m4 , m5 , m6 ) from the coordinates (r, , z) or (x, y, z) using
the following intermediate stages:




m3
d3
3
r



4 d4 m4
m5
d5
5
z
m6
m6

Forward kinematics

In this section we want to calculate the coordinates (r, , z) of the robots endpoint as a function of the motor values (m3 , m4 , m5 , m6 ). The way we follow
is:




3
d3
m3
r
4
d4
m4
.
5
d5
m5
z

m6
m6
10

Most of the calculations are very analogous with those in the inverse kinematics
section, therefore we keep them as short as possible. The lengths d3 , d4 and
d5 can be obtained by di = ui mi + vi (see 2.3). For the angle we become
= (m6 340)/129.87 (see also 2.3). Applying the cosine-rule in triangle
M4 N4 P4 (see section 2.2 and figure 4) we get
3 = arccos

e3 2 + f3 2 d3 2
2e3 f3

from which we compute 3 = 3 3 3 . Analogously we obtain in triangle

M 6 N6 P 6
 2

d5 e5 2 f5 2
5 = arccos
5 .
2e5 f5
The derivation of 4 is slightly different. First we compute
q
|M5 P5 | = a5 2 + f4 2 2a5 f4 cos 5
where 5 = 5 . Now we can obtain the angles
2

P6 d
P5 M5 = arccos

and

|M5 P5 | + a5 2 f4
2 |M5 P5 | a5
2

M5d
P5 N5 = arccos

|M5 P5 | + f4 2 d4 2
2 |M5 P5 | f4

!
!

with which we calculate 4 by observing that 4 = (P6 d

P5 M5 + M5d
P5 N5 ). Once
3 , 4 and 5 are known we obtain (see also [2] p. 49-51)
r

= a5 cos(5 ) + a4 cos(5 + 4 ) + a3 cos(5 + 4 + 3 )

= a5 sin(5 ) + a4 sin(5 + 4 ) + a3 sin(5 + 4 + 3 )

and finally r is determined by r = r + d.

References
[1] teach-robot instruction set. Microelectronic Kalms Gmbh, 76487 BadenBaden, Germany.
[2] L. Sciavicco and B. Siciliano. Modelling and Control of Robot Manipulators.
Advanced textbooks in control and signal processing. Springer, 2nd edition,
2000.

11