Inverse kinematics of 6-DOF manipulator with three intersecting axes

1
2
3
4
One has to understand that a single point could have di-
erent coordinates in diferent coordinate system. In certain
system the coordinates could be trivial, in other it shall be
calculated. Let us introduce the notation the point P has the
coordinates P
j
in j-th coordinate system.
For solving the task, one has to make few observations:
O
j
j
=
_
_
_
_
0
0
0
1
_
_
_
_
, (1)
P
i
= A
i
j
P
j
, (2)
T = A
0
1
(q
1
)A
1
2
(q
2
)A
2
3
(q
3
)A
3
4
(q
4
) . . . A
n1
n
(q
n
), (3)
A
i
j
= (A
0
i
)
1
T(A
j
n
)
1
, (4)
Let
A
i
j
=
_

_
t
i
jx
R
i
j
t
i
jy
t
i
jz
0 0 0 1
_

_
. (5)
Correspondingly rotation matrices for rotating vectors can be
composed and the order could changed by similar formulas.
Note that vectors are not in homogeneous coordinates. Vec-
tor coordinates can be calculated by well known formula from
vector algebra as a dierence of euclidean coordinates of two
points, dening the vector. The vector is denoted here as r.
r
i
= P
i
e2
P
i
e1
, (6)
P
i
e2
= P
i
e1
+r
i
, (7)
r
i
= R
i
j
r
j
, (8)
R = R
0
1
(q
1
)R
1
2
(q
2
)R
2
3
(q
3
)R
3
4
(q
4
) . . . R
n1
n
(q
n
), (9)
R
i
j
= (R
0
i
)
1
R(R
j
n
)
1
. (10)
ROBOTICS: Vladimr Smutn Slide 1, Page 1
Inverse kinematics of 6-DOF manipulator with three intersecting axes
...

...
1
2
3
4
Inverse kinematics
Inverse kinematics means, we are given position of the end
efector T and we have to nd joint coordinate vector q. We
will use in our reasoning following assumptions:
1. The manipulator is serial (open kinematic chain). This
transforms into the equation (3), where each transfor-
mation matrix is a function of one joint variable.
2. The manipulator has 6 DOF. With less DOF we will
not be able to reach arbitrary position and orientation
even in some limited 6D working space. With more DOF
one will get innitely many solutions within some 6D
working space.
3. The manipulator has three consecutive intersecting re-
volute joints near the end eector of the manipula-
tor. This is a common situation, the robot has a wrist
allowing any orientation of manipulated object.
To solve inverse kinematics of the 6-DOF serial manipula-
tor with three consecutive intersecting axes we could use
following reasoning. Let us have the three intersecting axes
4, 5, 6. The intersection of axes z
4
, z
5
, z
6
is marked as point
W. Using above simple formulas we can compute inverse ki-
nematics this way (next page). It is given T or equivalent
information e.g. coordinates of a center of gripper and its ori-
entation by quaternion. Let us calculate matrices A
i
i+1
for
i = 0..5 symbolically using D-H notation. For coordinates of
the point W then holds:
W
3
= O
3
3
=
_
_
_
_
0
0
0
1
_
_
_
_
, (11)
W
4
= O
4
4
=
_
_
_
_
0
0
0
1
_
_
_
_
, (12)
W
5
= O
5
5
=
_
_
_
_
0
0
0
1
_
_
_
_
. (13)
Let us dene auxiliary coordinate system O
7
by following con-
ditions:
W
7
= O
7
7
=
_
_
_
_
0
0
0
1
_
_
_
_
, (14)
R
7
6
=
_
_
1 0 0
0 1 0
0 0 1
_
_
= E (15)
ROBOTICS: Vladimr Smutn Slide 2, Page 2
that is A
5
7
represents only rotation and A
7
6
represents only
translation:
A
5
6
(q
6
) = A
5
7
(q
6
)A
7
6
, (16)
A
5
7
(q
6
) =
_

_
0
R
5
6
(q
6
) 0
0
0 0 0 1
_

_
, (17)
A
7
5
(q
6
) = (A
5
7
(q
6
))
1
=
_

_
0
R
5
6
(q
6
)
T
0
0
0 0 0 1
_

_
, (18)
A
7
6
=
_

_
1 0 0 t
7
6x
0 1 0 t
7
6y
0 0 1 t
7
6z
0 0 0 1
_

_
, (19)
A
6
7
= (A
7
6
)
1
=
_

_
1 0 0 t
7
6x
0 1 0 t
7
6y
0 0 1 t
7
6z
0 0 0 1
_

_
, (20)
A
5
6
= A
5
7
(q
6
)A
7
6
=
_
R
5
6
(q
6
) R
5
6
(q
6
)t
7
6
0 0 0 1
_
. (21)
A
6
7
is not a function of q
6
. A
5
6
is known symbolically from
D-H notation or the decomposition could be done from geo-
metrical analysis.
Inversion
A
6
5
(q
6
) = (A
5
6
(q
6
))
1
= (22)
=
_
R
5
6
(q
6
)
T
R
5
6
(q
6
)
T
R
5
6
(q
6
)t
7
6
0 0 0 1
_
=(23)
=
_
R
5
6
(q
6
)
T
t
7
6
0 0 0 1
_
= (24)
=
_

_
1 0 0 t
7
6x
0 1 0 t
7
6y
0 0 1 t
7
6z
0 0 0 1
_

_
_

_
0
R
5
6
(q
6
)
T
0
0
0 0 0 1
_

_
= (25)
= A
6
7
A
7
5
(q
6
). (26)
Note that A
6
5
(q
6
) could be easily factorized into two ma-
trices, one independent of q
6
.
Thus A
6
7
could be found by comparison in the above for-
mula. Transformations of the point W:
W
6
= A
6
7
W
7
, (27)
W
0
= TW
6
. (28)
Solving following equation for q
1
, q
2
, q
3
:
W
0
= A
0
1
(q
1
)A
1
2
(q
2
)A
2
3
(q
3
)W
3
, (29)
A
0
3
= A
0
1
(q
1
)A
1
2
(q
2
)A
2
3
(q
3
) (30)
is then known, as this is equivalent to 3-DOF manipulator
IKT solved in lab previously.
Then our task is
(A
0
3
)
1
T(A
7
6
)
1
= A
3
4
(q
4
)A
4
5
(q
5
)A
5
7
(q
6
). (31)
Note that q
4
, q
5
, q
6
are revolute joints. As O
4
= O
5
= O
7
,
we can forget about translation part of the transformations
matrices and simplify (31) into
(R
0
3
)
1
R
T
(R
7
6
)
1
= R
3
4
(q
4
)R
4
5
(q
5
)R
5
7
(q
6
). (32)
After simplication:
R
0T
3
R
T
= R
3
4
(q
4
)R
4
5
(q
5
)R
5
6
(q
6
). (33)
Solving (33) will give us q
4
, q
5
, q
6
. This can be achieved
by comparison with the rotation matrix of Euler angles. Then
the IKT is solved.
Cookbook
1. Factorize A
6
5
(q
6
) into A
6
7
A
7
5
(q
6
).
2. Identify t
7
6
and R
5
6
(q
6
).
3. Calculate W
0
= TA
6
7
_
_
_
_
0
0
0
1
_
_
_
_
.
4. Using lower 3 DOF inverse kinematics, solve (29) for
q
1
, q
2
, q
3
.
5. Solve equation (33) for q
4
, q
5
, q
6
.
ROBOTICS: Vladimr Smutn Slide 2, Page 3
Coordinate systems and transformations among them
T
B
0
A (q )
0
1 1
A (q )
2 2
1
A (q )
2
3 3
3
4 4 A (q )
4
5 5 A (q )
5
6 6 A (q )
A
6
T
S
B
S
1
S2
S3
S4
S5
S6
ST
A
7
6
S7
3
6 4 5 6 A (q , q , q )
0
3 1 3 2 A (q , q , q )
S
0
A (q )
5
7 6
A
1
2
3
4
ROBOTICS: Vladimr Smutn Slide 3, Page 4
How to check direct and inverse kinematic task

Ref
DKT
Student
IKT
Ref
DKT
Student
IKT
Ref
DKT
Student
DKT
X
X
{X} X
X
J
J
{J}
Euclidean distances
angular distances
Euclidean distances
angular distances
{J}
Test of DKT
Test of IKT distances
Test of IKT missing solutions
number of missing solutions

1
2
3
4
To check DKT and IKT, we need to have some kind of
ground truth. The ground truth could be:
1. IKT in robots control unit When we have a working
robot and we need to make its mathematical model we
could compare results from mathematical model with
real robot
2. Measured positions on the real robot When we are
designing control unit for new robot, we could measure
certain positions in space and then check, if tested IKT
will move the robot to the required positions.
3. DKT from mathematical model Mathematical mo-
del of DKT is for open kinematic chain much easier
to design as we have tools like DenavitHartenberg no-
tation. Then we could compare IKT and DKT in closed
loop. Similarly testing of DKT of parallel manipulators
could be done using IKT which could be easier to model.
In all cases we should have in mind, that we are not mathe-
maticaly proving the IKT is correct but testing it on the set of
positions. The fundamental problem is selection of these test
positions. The robot with 6-DOF have upto 16 congurations
(solutions) for each position. Further the robot is working in
6-D space with 2
6
quadrants. It is recommended to repre-
sent all those quadrants in testing set to get reasonable
certainty that IKT is not working just for e.g. positive values
in joint 3.
Another problem is to check, if all congurations are pro-
perly generated.
We recommend to use the following methodology, which
is also applied for check of your home assignments (see Fig.):
1. If there is ground truth or reliable DKT available, one
can compare tested IKT and ground truth DKT.
2. We insert positions into IKT which could or need not
be reachable. All generated solutions (dierent congu-
rations) are inserted into DKT and the results compared
with original positions.
3. We insert reachable joint coordinates into ground truth
DKT. The reachable joint coordinates usually could be
found in robots manual or from design parameters, they
often form n-dimensional interval. The output positions
is inserted into tested IKT and the output shall contain
orginal joint coordinates among other congurations.
One has to mention the method for evaluation of the dieren-
ces between reference and calculated solution. If we calculate
the dierence between positions we actually have two coordi-
nate transformation between base and end eector coordinate
systems. The translation part of the transformation could be
easily compared using standard euclidean distance. The ro-
tation part of the transformation is easy to compare in 2-D
case, where dierence between angles calculated modulo 2
is the rotation from one end-efector coordinate system to the
ROBOTICS: Vladimr Smutn Slide 4, Page 5
other. In spatial case we have two rotational matrices R
1
, R
2
.
We will calculate the rotation from reference end-efector co-
ordinate system to calculated end-efector coordinate system
R = R
1
R
1
2
, and then we convert it into the axis-angle de-
scription. The angle is then evaluated and is expected to be
close to zero.
ROBOTICS: Vladimr Smutn Slide 4, Page 6