Mechanism

and
Mechanism and Machine Theory 42 (2007) 66–81
Machine Theory
www.elsevier.com/locate/mechmt

A new and efficient algorithm for the inverse kinematics

of a general serial 6R manipulator
Manfred L. Husty *, Martin Pfurner, Hans-Peter Schröcker
University Innsbruck, Institute of Engineering Mathematics, Geometry and Computer Science, Technikerstraße 13, A6020 Innsbruck, Austria

Received 19 January 2006; received in revised form 7 February 2006; accepted 13 February 2006
Available online 17 April 2006

Abstract

In this paper a new and very efficient algorithm to compute the inverse kinematics of a general 6R serial kinematic chain
is presented. The main idea is to make use of classical multidimensional geometry to structure the problem and to use the
geometric information before starting the elimination process. For the geometric pre-processing we utilize the Study model
of Euclidean displacements, sometimes called kinematic image, which identifies a displacement with a point on a six dimen-
sional quadric S 26 in seven dimensional projective space P7. The 6R-chain is broken up in the middle to form two open 3R-
chains. The kinematic image of a 3R-chain turns out to be a Segre-manifold consisting of a one parameter set of 3-spaces.
The intersection of two Segre-manifolds and S 26 yields 16 points which are the kinematic images representing the 16 solu-
tions of the inverse kinematics. Algebraically this procedure means that we have to solve a system of seven linear equations
and one resultant to arrive at the univariate 16 degree polynomial. From this step in the algorithm we get two out of the
six joint angles and the remaining four angles are obtained straight forward by solving the inverse kinematics of two
2R-chains.
 2006 Elsevier Ltd. All rights reserved.

2000 MSC: 70B15; 53A17

Keywords: 6R manipulator; Inverse kinematics; Multidimensional geometry; Study representation; Kinematic mapping; Segre manifold

1. Introduction

The inverse kinematics of general 6R linkages attracted many excellent researchers of the kinematics com-
munity in the end 1980s and beginning 1990s of the last century. At that time this problem was considered to
be the most difficult among all open problems in kinematics. In 1988 Lee and Liang [1] came up with a solu-
tion which was not very transparent, so that most the time Raghavan and Roth’s [2] solution from 1990 is
cited in the textbooks. There were many attempts to improve the algorithm. For example, Ghazvini [3]

*
Corresponding author. Tel.: +43 512 507 6830; fax: +43 512 507 2709.
E-mail address: manfred.husty@uibk.ac.at (M.L. Husty).

0094-114X/$ - see front matter  2006 Elsevier Ltd. All rights reserved.
doi:10.1016/j.mechmachtheory.2006.02.001
 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81 67

reduced the problem to the solution of a generalized eigenproblem. An explicit enumeration and recognition
of all developments would be to extensive. An overview on the development over the years can be found in [4].
To the authors surprise the geometric structure of the problem was never really revealed. We always had the
feeling that the manipulations with the non-linear algebraic equations, describing the problem were somehow
as luck would have it, although with great intuition. To our opinion the method of kinematic mapping com-
bined with classical results of multidimensional geometry bear the tools to give the necessary insight to the
problem.
The paper is organized as follows: in Section 2 we introduce the mathematical and geometrical tools. We
recall the notation to describe serial chains and discuss the Study model of Euclidean displacements. In Sec-
tion 3 we introduce the constraint manifold of a spatial 3R-chain, which turns out to be the key to the solution
of the inverse kinematics. This constraint manifold is a well known construct in multidimensional geometry: a
so called Segre-manifold. Some of its properties are described in Section 4. In Section 5 we apply the results of
Sections 3 and 4 and give a new algorithm to solve the inverse kinematics of a general 6R-chain. In Appendix
A a numerical example for this algorithm is given.

2. Mathematical framework

2.1. Design parameters

A serial 6R manipulator is composed of a sequence of seven links connected by six revolute joints. The links
are numbered such that link zero constitutes the base of the robot and link six is the terminal link where the
end effector is located. Joint i connects link i and link i  1 and the variable joint parameter (rotation angle) is
denoted by ui. In order to define the relationship between the location of links, we assign a coordinate frame Ri
attached to link i, such that the zi -axes coincide with the axes of joint i, the xi -axes are aligned with the com-
mon normal of link i and i + 1. The yi -axes lie such that these three axes form a right-hand coordinate frame.
Then the relative displacement of R6 with respect to the fixed frame R0 can be computed by linking all relative
displacements of Ri with respect to Ri1 and is therefore given by the matrix:1
D ¼ M1  G1  M2  G2  M3  G3  M4  G4  M5  G5  M6  G6 ; ð1Þ
2 3 2 3
1 0 0 0 1 0 0 0
6 0 cosðu Þ  sinðu Þ 0 7 6a 1 0 0 7
6 i i 7 6 i 7
Mi ¼ 6 7; Gi ¼ 6 7
4 0 sinðui Þ cosðui Þ 0 5 4 0 0 cosðai Þ  sinðai Þ 5
0 0 0 1 d i 0 sinðai Þ cosðai Þ
for i = 1, . . . , 6, where ai is the twist angle between the z-axes of the links i  1 and i, ai is the offset distance
between the same axes and di is the translational distance of two common normals of the z-axes of link i with
the adjacent z-axes of links i  1 and i + 1. As usual the design parameters are called Denavit–Hartenberg
parameters (DH-parameters). Establishing the transformation matrices we have made use of the fact that
every serial chain can be brought to a position where all revolute axes are parallel to one plane. This config-
uration is used as home position of the chain. It is easy to see that this home position can be achieved by the
following algorithm: Choose the plane normal to the common perpendicular of the first two revolute axes and
fix the first one. Now the remaining manipulator axes are rotated about the second joint until the third axis is
parallel to the chosen plane. Now the first three axes are fixed and the next axis is rotated to a position parallel
to the chosen plane. This process is repeated until all axes are parallel to the plane. The result of this process is
shown in Fig. 1 in four different views. In this figure the yz-plane of R0 is chosen to be parallel to all axes. Note
that in the home position all Denavit–Hartenberg parameters can be seen in true shape: The common normals
of the rotation axes ai are parallel to the top view and the front view plane and the angles ai between the rota-
tion axes and the offsets di are in true shape in the side view. (Because the picture is described by side not right
view.)

1
Throughout the whole paper the European notation is used and the first coordinate is the homogenizing coordinate.
68 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81

Fig. 1. General 6R manipulator in home position, all axes parallel to yz-plane.

Furthermore this home pose shows that no serial manipulator is singularity free, because in this home posi-
tion all axes intersect the line at infinity of the chosen plane and belong therefore to a special linear complex.
The determinant of the Jacobian matrix of the manipulator vanishes in this position. It should be noted that
this home position – although being singular – does not at all restrict the generality of the following compu-
tations. It only reduces the complexity of the equations.

2.2. Study-model of SE(6)

Euclidean displacements D 2 SEð6Þ can be described by (see [5,6])

D : x0 ¼ Ax þ t; ð2Þ
where x 0 resp. x represent a point in the fixed resp. moving frame, A is a 3 · 3 proper orthogonal matrix and
t = [t1, t2, t3]T is the translation vector, connecting the origins of moving and fixed frame. Expanding the dual
quaternion representation [5, Section 3.3.2] and using an operator approach the matrix operator correspond-
ing to the normalized dual quaternion q = [x0, x1, x2, x3] + e[y0, y1, y2, y3] is given by
2 3
1 0 0 0
6 t x2 þ x2  x2  x2 2x x þ 2x x 2x3 x1 þ 2x0 x2 7
6 1 0 1 3 2 0 3 2 1 7
M :¼ 6 7; ð3Þ
4 t2 2x2 x1 þ 2x0 x3 x20 þ x22  x21  x23 2x0 x1 þ 2x3 x2 5
t3 2x0 x2 þ 2x3 x1 2x3 x2 þ 2x0 x1 x20 þ x23  x22  x21
 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81 69

where
t1 ¼ 2x0 y 1  2y 0 x1  2y 2 x3 þ 2y 3 x2 ;
t2 ¼ 2x0 y 2  2y 0 x2  2y 3 x1 þ 2y 1 x3 ; ð4Þ
t3 ¼ 2x0 y 3  2y 0 x3  2y 1 x2 þ 2y 2 x1 .
The point [x, y, z]T is transformed to [x 0 , y 0 , z 0 ]T according to
T T
½1; x; y; z ¼ M  ½1; x0 ; y 0 ; z0  .
The entries [xi, yi] in the transformation matrix M have to fulfill the quadratic identity
x0 y 0 þ x1 y 1 þ x2 y 2 þ x3 y 3 ¼ 0 ð5Þ
and at least one xi is different from 0. The lower right 3 · 3 sub-matrix of M is an element of the special
orthogonal group SO(3)+ and the xi are the Euler parameters. This representation of Euclidean displacements
is sometimes called Study representation and the parameters xi, yi are called Study parameters. This allows the
following multidimensional geometric interpretation: Eq. (5) defines a six dimensional quadric hyper-surface
in a seven dimensional projective space P7. This quadric S 26 is called Study quadric and serves as a point model
for Euclidean displacements. The quadric S 26 is of hyperbolic type and has the following properties:

(1) The maximal linear spaces on S 26 are three dimensional (generator spaces).
(2) Each tangent space cuts S 26 in a five dimensional cone.
(3) The generator space x0 = x1 = x2 = x3 = 0 is one of the 3-spaces mentioned above but it does not
represent regular displacements, because in this space all Euler parameters are zero. Therefore this space
has to be cut out of S 26 . A quadric with one generator space removed is called sliced.

A detailed treatment of more properties of S 26 can be found in [7, Chapter 10].

The mapping
j : D ! P 2 P7 ð6Þ
T T
Mðxi ; y i Þ ! ½x0 : x1 : x2 : x3 : y 0 : y 1 : y 2 : y 3  6¼ ½0 : 0 : 0 : 0 : 0 : 0 : 0 : 0
is called kinematic mapping and maps each Euclidean displacement D to a point P on S 26  P 7 .
Given a displacement D as in Eq. (2) it is straightforward to compute the Study parameters xi, yi. One can
use one of the formulas (7) to compute the Euler parameters xi directly from the 3 · 3 lower right sub-matrix
A = (aij)i, j=1,. . .,3 of M:
x0 : x1 : x2 : x3 ¼ 1 þ a11 þ a22 þ a33 : a32  a23 : a13  a31 : a21  a12
¼ a32  a23 : 1 þ a11  a22  a33 : a12 þ a21 : a31 þ a13
ð7Þ
¼ a13  a31 : a12 þ a21 : 1  a11 þ a22  a33 : a23 þ a32
¼ a21  a12 : a31 þ a13 : a23 þ a32 : 1  a11  a22 þ a33 .
These formulas are already due to Study [8]. If A is non-symmetric, we can always take the first formula of (7).
If A is symmetric, then it describes a rotation through an angle of p and the first formula fails. In this case we
can always resort to one of the three remaining formulas. The yi are given by
1 1
y 0 ¼  ðt3 x3 þ t2 x2 þ t1 x1 Þ; y 1 ¼  ðt3 x2  t2 x3  t1 x0 Þ;
2 2 ð8Þ
1 1
y 2 ¼  ðt3 x1 þ t1 x3  t2 x0 Þ; y 3 ¼  ðt3 x0 þ t2 x1  t1 x2 Þ.
2 2

3. Kinematic image of a 3R serial chain

The most important step in the proposed algorithm for the inverse kinematics of an open 6R-chain, devel-
oped in Section 5, is the decomposition of the 6R-chain into two 3R-chains. Therefore, at first we will study the
kinematic image of the 3R-chain and derive some of its geometric properties.
70 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81

Given the design (the DH-parameters) of an arbitrary 3R-chain we can compute the relative position of the
end-effector frame R with respect to a base frame R0 in dependence of three rotation angles u1, u2, u3. There-
fore, the constraint manifold of a 3R-chain, representing all poses the frame R can attain is a 3-manifold in the
kinematic image space.
It is known (see [7]) that the constraint manifold representing a 2R-chain in the kinematic image space is the
intersection of a 3-space T with the Study quadric S 26 . This fact has been used in [9] to derive a new method for
the synthesis of Bennett mechanisms. The intersection of S 26 and T is again a quadric surface.
If we fix one of the three revolute joints of the 3R-chain by fixing one parameter, say u3, we get a 2R-chain.
Its kinematic image lies in a 3-space T. Hence, the constraint manifold of the 3R-chain is the intersection of the
Study-quadric S 26 with a one parameter set of 3-spaces T(u3).
If we fix two of the three revolute joints of the 3R-chain by fixing the parameters u1 and u2, we get a pure
rotation about the third revolute axis. This rotation maps to a line l on the Study quadric. The line l itself
depends on the two parameters u1 and u2; therefore we write l = l (u1, u2). The points on l are parameterized
by u3. We have a closer look at this parametrization.
The parametric representation of the constraint manifold of the 3R-chain in kinematic image space, as com-
puted via (7) and (8), reads p(u1, u2, u3). We apply the half-tangent substitution vi = tan(ui/2), thus obtaining a
rational parametric representation p(v1, v2, v3) of degree four. Fixing v1 and v2 yields a rational parametrization
p(v3) of the straight line l (v1, v2).

Lemma 1. For any v1, v2, the parametric representation p(v3) of l(v1, v2) is linear in v3.

Proof. After half-tangent substitution, kinematic mapping and multiplication with the least common multiple
of the denominator polynomials, the homogeneous parametric representation p(v3) is polynomial and of
degree four – at least formally. Using a computer algebra system one finds that all eight coordinate polyno-
mials of p(v3) have a common divisor d(v3) of degree three. Hence, we can divide p(v3) by d(v3) without destroy-
ing its polynomial character. The resulting parametric representation is linear in v3. h
The 3-spaces T (v3) can be generated in the following way: We choose four fixed straight lines l(v1,i, v2,i),
parameterized by pi (v3) (i = 1, . . . , 4). Every 3-space T(v3) is spanned by p1(v3), . . . , p4(v3). On the other hand,
a straight line l (v1, v2) is the span of two points pj (v1, v2) 2 T(v3,j). The relation between the points pj (v1, v2) can
be extended to a unique projective transformation p : T(v3,1) ! T(v3,2) such that p(p1(v1, v2)) = p2(v1, v2) for all
v1, v2. Any straight line spanned by corresponding points x 2 T(v3,1) and p(x) 2 T(v3,2) intersects all 3-spaces
T(v3) and any four of these lines can be used to generate the 3-spaces T(v3).
The one parameter set of 3-spaces T(v3) generated as above is known in geometry as a Segre manifold. A
Segre manifold is also defined as topological product of two linear spaces, i.e., the manifold of all ordered
pairs of points of both spaces [10, p. 569].
So far, we always fixed the rotation angle v3 = tan(u3/2) of the third revolute joint, thus obtaining a Segre
manifold that will be denoted by SM3. Of course we can also fix v1 or v2. This yields two different one param-
eter sets of 3-spaces and two different Segre manifolds SM03 , SM003 . This fact will be important for the inverse
kinematics algorithm in Section 5 (see Remark 9).
We summarize the above considerations in
Theorem 2. The constraint manifold of a 3R-chain is the intersection of the Study quadric with a Segre manifold
SM3 ðSM03 , SM003 Þ, generated by two projectively coupled 3-spaces T1, T2  P7. The intersection of Ti with the Study
quadric S 26 is the kinematic image of a 2R-chain obtained by fixing the rotation angles u3 (u1, u2) of the 3R-chain.
A symbolic sketch of SM3 is depicted in Fig. 2.

Corrollary 3. The constraint manifold of a 3R-chain is the intersection of any two of the Segre manifolds SM3,
SM03 , SM003 .

Proof. We consider the Segre manifolds SM03 and SM003 . They are swept by 3-spaces T 0 (v1) and T00 (v2). The
intersection of T 0 (v1) and T00 (v2) is the straight line l(v1, v2). The union of all straight lines l (v1, v2) is precisely
the intersection of SM3 with the Study quadric S 26 , i.e., the constraint manifold of the 3R-chain. h
 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81 71

Fig. 2. Symbolic figure of SM3.

4. Different representations of the Segre manifold

In this section we describe different representations of the Segre manifold SM3. Having more than one rep-
resentation at hand is advantageous, since the representation can be chosen according to the specific problem.
We will present examples of this fact.

4.1. Parametric representation, span of points

We can view SM3 as generated by two projectively coupled 3-spaces or four projectively coupled straight
lines. This can be used to find a parametric representation of SM3.
Denote the four straight lines by li and let pi(v3) = T(v3) \ li. Because of Lemma 1 we may assume that pi(v3)
is linear in v3. Now the points of SM3 are described by
X
4
SM3: x ¼ ki pi ðv3 Þ;
i¼1

where [k1, k2, k3, k4]T is a homogeneous quartuple.

In this representation, the algebraic degree of SM3 is easily computed. It is defined as the number of inter-
section points of SM3 and a generic 3-space U  P7 [11]. Let U be the span of u1, . . . , u4. U and T(v3) intersect if
and only if

detðp1 ðv3 Þ; p2 ðv3 Þ; p3 ðv3 Þ; p4 ðv3 Þ; u1 ; u2 ; u3 ; u4 Þ ¼ 0. ð9Þ

(9) is a polynomial of degree four in v3. Hence there exist four intersection points of SM3 and T: The algebraic
degree of SM3 is four.

4.2. Algebraic equations

In order to find a set of algebraic equations of SM3 we fix two parameter values v3,1 and v3,2 and let pi,j =
pi (vj) for i = 1, . . . , 4 and j = 1, 2. In a projective coordinate frame with base points p1,1, . . . , p4,1, p1,2, . . . , p4,2,
we use homogeneous coordinate vectors [n0 : . . . : n3 : g0 : . . . : g3]T. In this frame, the algebraic equations of SM3
read
" #
ni gi
det ¼ 0; i; j 2 f0; . . . ; 3g ð10Þ
nj gj
72 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81

(see [10, p. 569]). Note that only three of these quadratic equations are independent. Therefore, SM3 is the
intersection of three hyper-quadrics. In order to find the equations of SM3 in the original coordinate frame
of P7, we have to apply the transformation y = Px where P is the matrix
P ¼ ½p1;1 ; . . . ; p4;1 ; p1;2 ; . . . ; p4;2 
to the equations in (10). There exist symmetric eight by eight matrices Aij such that Eq. (10) reads
½n0 ; . . . ; n3 ; g0 ; . . . ; g3   Aij  ½n0 ; . . . ; n3 ; g0 ; . . . ; g3 T ¼ 0.
The transformed equations are
½n0 ; . . . ; n3 ; g0 ; . . . ; g3   ðP1 ÞT Aij P1  ½n0 ; . . . ; n3 ; g0 ; . . . ; g3 T ¼ 0. ð11Þ

Remark 4. It is possible to compute symbolically the system of algebraic equations of SM3 without specifying
the DH-parameters of the 3R-robot. On current PCs with standard hard- and software this pre-processing can
be done in short time.

4.3. Intersection of hyper-planes

A 3-space in the seven dimensional projective kinematic image space is geometrically determined by inter-
secting four hyper-planes H1, . . . , H4. Algebraically this means it is given by four linear equations. We will
describe every 3-space T(v3) in this way. The Segre manifold is then the intersection of four hyper-planes, each
depending on the parameter v3. This representation will be used in the proposed algorithm for the inverse kine-
matics of an open 6R-chain in Section 5.
For each of the four hyper-planes Hj (v3) four points pi (v3) are given; but they do not determine Hj (v3).
Because seven points span a hyper-plane in P7 we need three more points aj,1, aj,2, aj,3 2 Hj (v3) to compute
a linear equation of Hj (v3). In principle, the points aj,k can be taken anywhere in P7 but it makes sense to
choose them such that the resulting hyper-plane equations become as simple as possible. Without loss of
generality we take the three points such that aj;k 2 SM3 \ S 26 . We choose the points aj,k independent of v3;
there exist fixed parameter values vjk jk
3 such that aj;k 2 T ðv3 Þ.
The points pi (v3) and aj,k have coordinate vectors
T T
pi ðv3 Þ ¼ ½pi0 ; . . . ; pi7  ; aj;k ¼ ½ajk jk
0 ; . . . ; a7  .

Note that the coordinates of pi depend linearly on v3, although this does not reflect in our notation. The
equation of a hyper-plane Hj (v3) is given by the Grassmannian determinant
2 3
x0 x1 x2 x3 y 0 y 1 y 2 y 3
6p 7
6 10 p11 p12 p13 p14 p15 p16 p17 7
6 7
6 .. .. 7
6 . . 7
6 7
H j ðv3 Þ: det 6
6 p40 p41 p42 p43 p44 p45 p46 p47 7 ¼ 0.
7 ð12Þ
6 j1 j1 j1 j1 j1 j1 j1 j1 7
6 a0 a1 a2 a3 a4 a5 a6 a7 7
6 7
6 j2 7
4 a0 aj2
1 aj2
2 aj2
3 aj2
4 aj2
5 aj2
6 aj2
7 5

aj3
0 aj3
1 aj3
2 aj3
3 aj3
4 aj3
5 aj3
6 aj3
7

We denote the coefficients of the hyper-plane equation (12) by cjl, i.e.,

H j ðv3 Þ: cj0 x0 þ cj1 x1 þ cj2 x2 þ cj3 x3 þ cj4 y 0 þ cj5 y 1 þ cj6 y 2 þ cj7 y 3 ¼ 0. ð13Þ
The coefficients cjl are polynomials of degree four in v3 and can be interpreted as homogeneous coordinates for
the hyper-plane Hj. Because of the special choice of the points aj,k, the polynomials cjl have the common
divisor
g ¼ ðv3  v3j1 Þðv3  v3j2 Þðv3  v3j3 Þ. ð14Þ
 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81 73

Dividing the polynomials cjl by g yields hyper-plane coordinates for Hj (v3) that depend linearly on v3. We state
this as
Theorem 5. The Segre manifold SM3 is the intersection of four one parameter pencils of hyper-planes Hj (v3).
The hyper-plane coordinates of Hj (v3) depend linearly on v3.

Remark 6. It is even possible to compute symbolically the hyper-plane coordinates of Hj (v3) without speci-
fying the DH-parameters of the 3R-robot. Computed once the equations can be taken for every possible
design.
It is not necessary to compute the common divisor g of Eq. (14) by an appropriate choice of the points aj,k.
If we let u3 = p, i.e., v3 = 1, the determinant (12) will be only of degree one in v3. This can be seen as follows:
We divide Eq. (13) by v43 . In the limit v3 ! 1 all but the leading coefficients of cjk vanish. Hence, v3 = 1
can be regarded as a common zero of multiplicity three of the coefficients of the hyper-plane coordinates of Hj.

5. Discussion of the inverse kinematics of general 6R manipulators

In this section we show how the constraint manifolds of 3R-chains can be used to solve the inverse kine-
matics of a general open 6R manipulator. Recall that in the inverse kinematic problem of a serial chain the
design and a pose of the end-effector of the manipulator is known. The rotation angles ui of the revolute joints
have to be computed. To apply the theory developed in the last sections we break up the link between the third
and the fourth revolute joint of the 6R and attach two copies of a coordinate frame RL = RR (the ‘‘left’’ and
the ‘‘right’’ frame) in the middle of the common normal of these two joints such, that the twist angle between
the z-axis of R3 and RL = RR is a23 (Fig. 3). Then we compute the direct kinematics for the first half of the 6R
manipulator, which is now a 3R-chain, with the end-effector frame RL. The pose of RL with respect to R0 is
given by
2 3
1 0 0 0
6 a3 1 0 0 7
6 a  7
T1 ¼ M1  G1  M2  G2  M3  G31 ; G31 ¼ 6 2 a  7. ð15Þ
4 0 0 cos 23  sin 23 5
a3  a3 
d 3 0 sin 2 cos 2

Fig. 3. Cutting of the 6R into two 3R serial chains.

74 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81

When the end-effector of the 6R manipulator is fixed we can do the same with the second part of the 6R but in
the opposite direction. This means that for this 3R manipulator R6 is the base and RR is the end-effector frame.
Therefore, the pose of RR with respect to R6 is given by
2 3
1 0 0 0
6 a3 1 0 0 7
62 7
T2 ¼ A  G1
6  M 1
6  G1
5  M 1
5  G1
4  M 1
4  G 1
32 ; G 32 ¼ 6  a
  a
 7; ð16Þ
4 0 0 cos 23  sin 23 5
a 3  a3 
0 0 sin 2 cos 2
where A represents the coordinate transformation from R6 to R0. In some discrete poses RL and RR have to
coincide. From literature it is well known that there will be up to 16 such real poses when the given 6R-chain is
general. They are found as the intersection
S 26 \ SM3;L \ SM3;R ;
where

• S 26 is the Study quadric.

• SM3,L is the kinematic image of the left 3R-chain and
• SM3,R is the kinematic image of the right 3R-chain.

SM3,L is a Segre manifold in P7. In Section 4.3 we saw that SM3,L is the intersection of four one-parameter
sets of hyper-planes. The same is true for SM3,R. Hence, we have to intersect eight one parameter sets of
hyper-planes with S 26 . We summarize these results in
Theorem 7. Geometrically the inverse kinematics of a general 6R serial chain is equivalent to the intersection of
eight one parameter sets Hi (v3), Hi+4(v6) of hyper-planes with S 26 (i = 1, . . . , 4).
An investigation of the algebraic structure of the nine equations reveals the non-linearity of the problem.
We have eight hyper-plane equations Hi (v3) = 0, Hi+4(v6) = 0, i = 1, . . . , 4 and Eq. (5) of S 26 . The equations Hi
are linear in xi, yi and bilinear in xiv3 and yiv3 resp. xiv6 and yiv6. The solution algorithm of this system is
straight forward.
At first we normalize the Study parameters by setting one suitable coordinate equal to one (at least one
Study parameter has to be non-zero!). The remaining seven Study parameters are solved linearly from seven
arbitrary Hi equations, say H1, . . . , H7. As result we obtain all Study parameters depending on the two param-
eters v3 and v6, i.e., xi = xi (v3, v6), yi = yi (v3, v6). Substituting xi and yi in the one remaining equation H8 and in
Eq. (5) we obtain two non-linear algebraic equations E1(v3, v6) = 0, E2(v3, v6) = 0.
Remark 8. The vanishing of E1 is the condition for intersecting three-spaces TL(v3) and TR(v6) of the left and
the right Segre manifold. Hence, it can also be written as
det½p1L ; p2L ; p3L ; p4L ; p1R ; p2R ; p3R ; p4R  ¼ 0;

where piL = piL(v3) and pj R = pj R(v6) are four points that span TL(v3) and TR(v6), respectively.
The resultant of E1 and E2 with respect to one unknown yields a univariate polynomial of degree 48 in the
remaining unknown. This polynomial factors into a univariate polynomial P1 of degree 16, in a polynomial
8
P2 of degree 8 squared and in a polynomial ð1  v2i Þ , i = 3 or i = 6 that comes from the half tangent substi-
tution. P1 yields the solution of the system and P2 comes from the determinant of the linear system that has
been solved before. P2 would lead to the trivial solution [0 : 0 : 0 : 0 : 0 : 0 : 0 : 0 : 0]T and therefore it has to be non-
zero.
We solve P1 for the unknown and obtain 16 solutions over C. The solution of the other unknown we obtain
by back substitution in E1 and E2. Solving both equations and comparing the solutions yields one common
solution of the system. Note that the solutions of the unknowns v3 and v6 are already the algebraic values
of the joint angles of joint three and six. Having the values of v3 and v6 the poses of the middle coordinate
system RL = RR can be computed with the corresponding Study parameters.
 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81 75

The remaining task is simple: We have to compute the joint angles of two 2R-chains, given the pose of the
end effector. We know the matrices T e 1 ðu1 ; u2 Þ, u3 = const. and T
e 2 ðu4 ; u5 Þ, u6 = const. that describe the motion
of these chains. Therefore we have to compare the entries of the two motion matrices with the entries of the
matrix that describes the pose of the end effector of the 2R-chains. This leads to an over-constrained set of equa-
tions for the four unknown angles of the chains. It turns out that (in the general case) we get for every pair of
angles u3 and u6 corresponding to the algebraic values v3 and v6 exactly one quadruple of angles u1, u2, u4 and u5.
The whole algorithm yields 16 sets of angles for the revolute joints of the general 6R serial chain and there-
fore solves the inverse kinematics problem.
Algorithm 1 (Inverse kinematics of a general 6R-chain).
Input: 6R-chain by its DH-parameters, the pose of an end-effector.
Output: 16 sets of rotation angles that yield the given end-effector pose.

(1) According to Section 5, Eqs. (15) and (16) compute the pose of RL (the ‘‘left middle frame’’) of the 6R-
chain, with respect to R0 (the ‘‘left’’ 3R-chain) and the pose of RR (the ‘‘right middle frame’’) with respect
to R6 (the ‘‘right’’ 3R-chain).
(2) Compute four parametric representations p1(v3), . . . , p4(v3) of straight lines in the kinematic image of the
left 3R-chain (see Section 3). Do the same for the right 3R-chain, obtaining four parametric representa-
tions q1(v6), . . . , q4(v6).
(3) Use the parametric representations pi (v3) and qj (v6) to compute algebraic equations for the hyper-planes
H1(v3), . . . , H4(v3) and H5(v6), . . ., H8(v6) according to Eq. (12).
(4) Intersect the hyper-planes H1(v3), . . . , H4(v3) and H5(v6), . . . , H7(v6). This yields a point r(v3, v6), depend-
ing on the two parameters v3 and v6.
(5) Substitute r(v3, v6) in the equation of H8(v6) and Eq. (5) of the Study quadric. This yields two algebraic
equations E1, E2 in v3 and v6.
(6) Compute the resultant R of E1 and E2 with respect to v3. The resultant is a polynomial that can be
written as
R ¼ ð1 þ v26 Þ8  P1 ðv6 Þ  P2 ðv6 Þ2 .
In this equation, P2 ðv6 Þ and P1 ðv6 Þ are polynomials of respective degree eight and 16. The roots
v6,1, . . . , v6,16 of P1 ðv6 Þ are the possible solutions for v6 and lead directly to the 16 joint angles via
u6,i = 2 arctan v6,i.
(7) Insert v6,i into E1 and E2 and compute the unique common solution v3,i of the resulting algebraic
equations in v3.
(8) Let ri = r(v3,i, v6,i). The points ri are the kinematic images of those poses where RL = RR.
(9) Solving the inverse kinematics of the remaining ‘‘left’’ and ‘‘right’’ 2R-chain yields all missing joint
angles.

Remark 9. It is possible that the Segre manifold SM3 is not suited for the algorithm. If, for example, the first
and second revolute axes are parallel or intersecting, the two parameter motion obtained by fixing u3 of the
‘‘left’’ 3R-chain is planar or spherical. As a consequence, SM3 is completely contained in S 26 . In this case
we have to replace SM3 by one of the Segre manifolds SM03 or SM003 , obtained by fixing the first or the second
revolute angle.2

Remark 10. If all three revolute axes of a 3R-chain are intersecting (wrist partitioned manipulator) or paral-
lel, the Segre manifold SM3 degenerates to a single 3-space on the Study quadric S 26 . In this case our algorithm
is even simpler: we just have to intersect four one parameter sets of hyper-planes with a fixed 3-space.

2
Note that some of the well known industrial manipulators have this feature. We emphasize that nevertheless the algorithm works. We
just have to take another Segre manifold.
76 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81

6. Conclusion

A new algorithm to compute the inverse kinematics of general 6R-chains was presented. Using multidimen-
sional geometry and classical theory of Segre manifolds for the first time the geometric nature of the problem
was revealed. The efficiency of the developed theory is demonstrated in a numerical example in Appendix A. It
is believed that this algorithm can be successfully applied to all different 6R-chains and that it can be
completely automatized. This step is subject to further research.

Appendix A. Numerical example

In this appendix we illustrate our algorithm at hand of a numerical example.

A.1. Parameters of the given manipulator

The design of the 6R robot is given by the Denavit Hartenberg parameters:

p p p p p p
a1 ¼  ; a2 ¼ ; a3 ¼ ; a4 ¼  ; a5 ¼  ; a6 ¼ ; a1 ¼ 10;
6 6 3 6 6 4
a2 ¼ 100; a3 ¼ 150; a4 ¼ 20; a5 ¼ 5; a6 ¼ 20;
d 1 ¼ 100; d 2 ¼ 50; d 3 ¼ 50; d 4 ¼ 50; d 5 ¼ 20; d 6 ¼ 10.

To verify the algorithm above we compute one pose of the end effector with a set of ‘‘nice’’ parameters of the
revolute joints. We set
p p p p p p
u1 ¼  ; u2 ¼ ; u3 ¼  ; u4 ¼ ; u5 ¼ ; u6 ¼  ðA:1Þ
6 2 3 2 6 6

and obtain the matrix A describing the pose of the end effector:
2 3
1 0 pffiffi
0pffiffi 0
pffiffi pffiffi
pffiffi pffiffi
6 3395 þ 7005 3  59 3 þ 177  2ð114 3þ433Þ 2ð66 3115Þ 7
6 128 64 512 256 1024
pffiffi pffiffi
1024
pffiffi pffiffi
7
A¼6 pffiffi pffiffi
6 4155 3 þ 5455 57 3 þ 39 2ð39 3þ122Þ
7
2ð21 3650Þ 7.
4 128 64 256 512 1024 1024 5
pffiffi pffiffi pffiffi pffiffi pffiffi pffiffi
1845 3 695 15 3 137 2ð55 3246Þ 2ð59 3þ90Þ
64
 32 128
þ 256
 512 512

A.2. Constraint manifold of the first 3R

In the first step of the algorithm the constraint manifold of the first 3R is computed. The matrix T1 (Eq.
(15)) describes the relative position of RL with respect to R0 and depends on the first three joint parameters
u1, u2 and u3. According to the algorithm above one 3-space of the constraint manifold of this 3R-chain is
spanned by four points, corresponding to the kinematic images of four poses of the end effector of this chain.
We assign values for u1 and u2, perform half tangent substitution to get rid of the trigonometric functions and
obtain four one parameter sets of kinematic image points:
pffiffiffi pffiffiffi pffiffiffi pffiffiffi
p1 ð0; 0; v3 Þ ¼ ½4  2 3; 2; 2v3 ; 4v3  2v3 3; 185  400v3  200v3 3; 370 þ 185 3  100v3 ; 200
pffiffiffi pffiffiffi pffiffiffi
þ 50 3  70v3  35v3 3; 350 þ 200 3 þ 35v3 T ;
 p  pffiffiffi pffiffiffi pffiffiffi pffiffiffi pffiffiffi pffiffiffi pffiffiffi
p2 0; ; v3 ¼ ½4v3  2v3 3 þ 4 3 þ 6; 2v3 þ 2 3; 2v3 3 þ 2 3 þ 2; 4v3 3 þ 2 3 þ 2 þ 6v3 ; 585v3
3
pffiffiffi pffiffiffi pffiffiffi pffiffiffi pffiffiffi
þ 300 þ 385 3 þ 400v3 3; 30v3  455  270 3 þ 85v3 3; 105v3  315  365 3
pffiffiffi pffiffiffi pffiffiffi T
þ 120v3 3; 165v3 3  185 3  435 þ 350v3  ;
 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81 77
p  pffiffiffi pffiffiffi pffiffiffi pffiffiffi
p3 ; 0; v3 ¼ ½4v3  2v3 3 þ 4 þ 2 3; 2v3 þ 2; 2 þ 2v3 ; 4 þ 4v3 þ 2 3 þ 2v3 3; 435v3 þ 535
2
pffiffiffi pffiffiffi pffiffiffi pffiffiffi pffiffiffi
þ 200 3 þ 200v3 3; 30v3  170  135 3  35v3 3; 170v3  570  235 3
pffiffiffi pffiffiffi pffiffiffi T
þ 35v3 3; 365v3  165  200 3 þ 200v3 3 ;
p p  pffiffiffi pffiffiffi pffiffiffi pffiffiffi pffiffiffi pffiffiffi
p4 ; ; v3 ¼ ½2  ð8 þ 4 3Þv3 ; 2 3; 4v3 þ 4 þ 2 3; 4 3 þ 6; 70v3 þ 670 þ 235 3; 95 þ 130 3
2 2
pffiffiffi pffiffiffi pffiffiffi pffiffiffi pffiffiffi
 100v3 3; ð140 þ 70 3Þv3  635  300 3; ð400 3 þ 700Þv3 þ 165 3 þ 300T .

For a constant parameter value v3 these points span one 3-space T(v3) in the kinematic image space. To
describe this 3-space we intersect four hyper-planes which are on this 3-space. To obtain one of these
hyper-planes we add three arbitrary points aj,1, aj,2 and aj,3, outside of the 3-space and compute the
Grassmann determinant to get the equation Hj (v3). The coefficients of Hj (v3) are polynomials of degree four
in v3. In order to get a simpler equation of Hj (v3) we choose the points aj,1, aj,2 and aj,3 in the fixed 3-space
T(1). Points in T(1) are found by substituting arbitrary values of u1 and u2 and u3 = p in T1 (because
v3 ! 1 corresponds to u3 ! p in the half tangent substitution). Then the Grassmann determinant immedi-
ately yields equations Hj (v3) that are linear in v3.
 p  pffiffiffi pffiffiffi pffiffiffi pffiffiffi pffiffiffi pffiffiffi pffiffiffi T
a1;1 ¼ p 0; ; p ¼ ½4  2 3; 2; 2 3; 6 þ 4 3; 585 þ 400 3; 30 þ 85 3; 105 þ 120 3; 350 þ 165 3 ;
p 3  pffiffiffi pffiffiffi pffiffiffi pffiffiffi pffiffiffi pffiffiffi T
a1;2 ¼ p ; 0; p ¼ ½8  4 3; 4; 4; 4 3 þ 8; 870 þ 400 3; 60  70 3; 340 þ 70 3; 730 þ 400 3 ;
p2
p  pffiffiffi pffiffiffi pffiffiffi pffiffiffi
a1;3 ¼ p ; ; p ¼ ½16  8 3; 0; 8; 0; 140; 200 3; 280 þ 140 3; 1400 þ 800 3T ;
2 p2 p  pffiffiffi pffiffiffi pffiffiffi
a2;1 ¼ p  ; ; p ¼ ½2; 2  2 3; 2  2 3; 2 3  4;
3 6 pffiffiffi pffiffiffi pffiffiffi pffiffiffi
 355  210 3; 40  110 3; 160 þ 40 3; 110 þ 75 3T ;
p p  pffiffiffi pffiffiffi pffiffiffi
a2;2 ¼ p ;  ; p ¼ ½2; 2 þ 2 3; 2  2 3; 2 3  4;
6 3 pffiffiffi pffiffiffi pffiffiffi pffiffiffi T
 305  260 3; 60  90 3; 90  110 3; 160 þ 25 3 ;
 p p  pffiffiffi pffiffiffi pffiffiffi pffiffiffi
a2;3 ¼ p  ;  ; p ¼ ½8  4 3; 2 þ 2 3; 2  2 3; 8  4 3;
6 3 pffiffiffi pffiffiffi pffiffiffi pffiffiffi T
 730  450 3; 165  115 3; 75  105 3; 820 þ 400 3 .
 p  pffiffiffi pffiffiffi pffiffiffi
a3;1 ¼ p  ; p; p ¼ ½12  6 3; 6; 2 3; 4 3 þ 6;
3 pffiffiffi pffiffiffi pffiffiffi pffiffiffi T
555 þ 350 3; 60  45 3; 45 þ 180 3; 1050 þ 615 3 ;
p  pffiffiffi pffiffiffi pffiffiffi
a3;2 ¼ p ; p; p ¼ ½14  8 3; 4 þ 2 3; 2; 2 3  4;
6 pffiffiffi pffiffiffi pffiffiffi T
 215 3  380; 105  110 3; 120 þ 85 3; 1285 þ 750 ;
 p p  pffiffiffi pffiffiffi pffiffiffi pffiffiffi
a3;3 ¼ p  ;  ; p ¼ ½8  4 3; 2 þ 2 3; 2  2 3; 8  4 3;
6 3 pffiffiffi pffiffiffi pffiffiffi pffiffiffi T
 730  450 3; 165  115 3; 75  105 3; 820 þ 400 3 ;
 p  pffiffiffi pffiffiffi pffiffiffi
a4;1 ¼ p  ; 0; p ¼ ½2 3  4; 2; 2 3; 6  4 3;
3 pffiffiffi pffiffiffi pffiffiffi pffiffiffi T
 565  400 3; 70  135 3; 5  70 3; 400 þ 235 3 ;
 p  pffiffiffi pffiffiffi pffiffiffi
a4;2 ¼ p 0;  ; p ¼ ½2 3  4; 2; 2 3; 6  4 3;
3 pffiffiffi pffiffiffi pffiffiffi pffiffiffi T
 615  400 3; 30  115 3; 105  20 3; 350 þ 235 3 ;
 p  pffiffiffi pffiffiffi pffiffiffi pffiffiffi T
a4;3 ¼ p  ; 0; p ¼ ½2; 4 þ 2 3; 2; 2 3  4; 330  235 3; 135; 130  135 3; 235 .
6
78 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81
pffiffiffi pffiffiffi
H 1 ðv3 Þ ¼ x0 ð879820000 3v3 þ 7997831335 3 þ 18615630775  2458446230v3 Þ
pffiffiffi pffiffiffi
þ x1 ð15557665485 3  10289261725 þ ð882841230  1504944900 3Þv3 Þ
pffiffiffi
þ x2 ð 3ð8765324530v3  10282046735Þ  10864007140v3  15539094485Þ
pffiffiffi
þ x3 ð 3ð14265570715 þ 17086648130v3 Þ  20362019865 þ 948245440v3 Þ
pffiffiffi pffiffiffi
þ y 0 ð81665352v3  21097838 3  178631836 3v3 þ 28205302Þ
pffiffiffi pffiffiffi
þ y 1 ð21097838  192562844 3v3 þ 148938168v3  18119382 3Þ
pffiffiffi
þ y 2 ð163785002 3  248011590 þ 41281724v3 Þ
pffiffiffi pffiffiffi
þ y 3 ð42195676v3 þ 193478670 þ 5042960 3v3 þ 137114098 3Þ ¼ 0;
pffiffiffi
H 2 ðv3 Þ ¼ x0 ð 3ð111014692455  36908859090v3 Þ  163990921360  54091774350v3 Þ
pffiffiffi pffiffiffi
þ x1 ð11731078460v3  952551340 3  4349708810 3v3  27897733735Þ
pffiffiffi
þ x2 ð 3ð11716981360v3 þ 84696082225Þ  40685203670v3 þ 140549969160Þ
pffiffiffi
þ x3 ð 3ð83471736350v3 þ 25596865180Þ  153129765650v3 þ 11397817355Þ
pffiffiffi pffiffiffi
þ y 0 ð824350144 3v3  245876200 3  414555694 þ 1461521816v3 Þ
pffiffiffi pffiffiffi
þ y 1 ð148507876 3v3  381217586 3 þ 727713440v3  245876200Þ
pffiffiffi
þ y 2 ð364548532v3  507999094  48318352 3Þ
pffiffiffi pffiffiffi
þ y 3 ð805014846 3  397886640 3v3  491752400v3  1600381936Þ ¼ 0;
pffiffiffi pffiffiffi
H 3 ðv3 Þ ¼ x0 ð4214917835 3 þ 8425162415 3v3  17698709765  3644110880v3 Þ
pffiffiffi pffiffiffi
þ x1 ð3971227815v3  22588758935 3 þ 3496621050 3v3 þ 26010131165Þ
pffiffiffi
þ x2 ð 3ð19962750315v3  9461671465Þ þ 33061744220v3 þ 18084770365Þ
pffiffiffi
þ x3 ð 3ð20143734350v3  34859357735Þ þ 26740524685v3 þ 48621553935Þ
pffiffiffi pffiffiffi
þ y 0 ð306339240 3v3  53519738 3 þ 230723422  460427418v3 Þ
pffiffiffi pffiffiffi
þ y 1 ð373401382 3v3 þ 11013742 3  625210164v3  53519738Þ
pffiffiffi
þ y 2 ð98841098v3 þ 790315782  465774702 3Þ
pffiffiffi pffiffiffi
þ y 3 ð43513018 3 þ 120868582 3v3  107039476v3  146903778Þ ¼ 0;
pffiffiffi pffiffiffi
H 4 ðv3 Þ ¼ x0 ð2270v3 þ 1000 3v3 þ 3600 3 þ 7125Þ
pffiffiffi pffiffiffi
þ x1 ð1950  270 3v3  40v3 þ 75 3Þ
pffiffiffi pffiffiffi
þ x2 ð75 3v3  6940  3220 3 þ 1950v3 Þ
pffiffiffi pffiffiffi
þ x3 ð1220 þ 3600 3v3 þ 7125v3  1000 3Þ
pffiffiffi pffiffiffi
þ y 0 ð36 3v3  72v3 þ 10 3 þ 20Þ
pffiffiffi pffiffiffi
þ y 1 ð10  36v3 Þ þ y 2 ð36 þ 10v3 Þ þ y 3 ð36 3 þ 72 þ 10 3v3 þ 20v3 Þ ¼ 0.

The intersection of these four pencils of hyper-planes with the Study quadric is the constraint manifold SM3 of
the first half of the 6R serial chain.
 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81 79

A.3. Constraint manifold of the second 3R

To get the constraint manifold for the second half of the 6R-chain we have to perform the same procedure
with matrix T2 instead of T1. Because of limit of space we do not display all the long equations:
pffiffiffi pffiffiffi
q1 ð0; 0; v6 Þ ¼ ½73726v6 þ 147452 þ 60738 3  6112 3v6 ;
pffiffiffi pffiffiffi
77164v6  125678 þ 129880 3 þ 39346 3v6 ;
pffiffiffi pffiffiffi
121094v6  22156  41638 3  93208 3v6 ;
pffiffiffi pffiffiffi
 28268v6 þ 29414 þ 56536 3  138666 3v6 ;
pffiffiffi pffiffiffi
 6929480v6 þ 28673875  13788290 3 þ 1991175 3v6 ;
pffiffiffi pffiffiffi
7595115v6  7617080  8194855 3 þ 6728930 3v6 ;
pffiffiffi pffiffiffi
 24921680v6  9480285 3v6 þ 65895 þ 4944990 3;
pffiffiffi pffiffiffi T
15725985v6 þ 3220260 þ 6158795 3 þ 51570 3v6  ;
 p  p  p p 
T T T
q2 ¼ 0; ; v6 ¼ ½    ; q3 ¼ ; 0; v6 ¼ ½    ; q4 ; ; v6 ¼ ½    .
3 2 2 2
The other points that span the hyper-planes are
p  h pffiffiffi pffiffiffi pffiffiffi
q1;1 ¼ q ; p; p ¼ 4328 þ 1700 3; 1100 þ 808 3; 1712  92 3;
3 pffiffiffi pffiffiffi pffiffiffi
 1316  256 3; 112765  137375 3; 7505  143365 3;
pffiffiffi pffiffiffi iT
325905  163005 3; 54195 3  5335 ;
 p p   p p 
q1;2 ¼ q  ; ; p ; q1;3 ¼ q  ; ; p ;
 p3 3  p 3 p 3   p
p p 
q2;1 ¼ q  ; ; p ; q2;2 ¼ q ;  ; p ; q2;3 ¼ q  ;  ; p ;
 p3 6 6 3   p6 p3 
p  p
q3;1 ¼ q  ;  ; p ; q3;2 ¼ q ; p; p ; q3;3 ¼ q  ;  ; p ;
 p3 6  6p   p6 3
q4;1 ¼ q  ; 0; p ; q4;2 ¼ q 0;  ; p ; q4;3 ¼ q  ; 0; p ;
3 3 6
pffiffiffi pffiffiffi pffiffiffi
H 5 ðv6 Þ ¼ 1366845x1 3  16788y 1 þ 1592995x2 3  14046y 2 þ 48y 0 3
pffiffiffi pffiffiffi
 9992y 2 3  698135x1 þ 2328655x2 þ 14046y 3 3 þ 28092y 0 v6  3091215x2 v6
pffiffiffi pffiffiffi pffiffiffi pffiffiffi
 1110595x0 3  169985x0 v6 3  458955x1 v6 3  1761015x2 v6 3 þ 455305x0
pffiffiffi pffiffiffi
þ 12230y 0 þ 20044y 3  986y 1 3  1036125x1 v6 þ 1632925x3 þ 630255x3 3
pffiffiffi
 774385x0 v6  8466y 3 v6 þ 4636y 2 v6 þ 4966y 1 v6 þ 15018y 0 3v6 þ 340695x3 v6
pffiffiffi pffiffiffi pffiffiffi
 381055x3 3v6  8370y 2 3v6  6608y 3 3v6 ¼ 0;
H 6 ðv6 Þ ¼    ¼ 0; H 7 ðv6 Þ ¼    ¼ 0; H 8 ðv6 Þ ¼    ¼ 0.

A.4. Pose of RL = RR

Now, eight one parametric pencils of hyper-planes (H1(v3), H2(v3), H3(v3), H4(v3) and H5(v6), H6(v6), H7(v6),
H8(v6)) are given and we have to intersect these eight pencils with S 26 to get the poses where RL and RR coin-
cide. Intersecting seven of the hyper-planes means solving seven linear equations in xi, yi, i = 0, . . . , 3. Remem-
ber, xi, yi are homogeneous, so one of the Study parameters can be normalized to one. Substituting the
solutions in the remaining eighth hyper-plane and in S 26 yields two non-linear equations E1(v3, v6) and
E2(v3, v6) which are of bi-degree four resp. six.
80 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81

Equation E1 has a geometric meaning. It gives us the parameter values, for which the two Segre manifolds
(or the eight hyper-planes) intersect. So it can also be generated by the determinant of the matrix:
pffiffiffi
E1 ¼ det½p1 ðu3 Þ; . . . ; p4 ðu3 Þ; q1 ðu6 Þ; . . . ; q4 ðu6 Þ ¼ 127961043861390176 3
pffiffiffi
þ 695430062705310720 3v3  869347902378014720v3 v36  ½. . . 45 terms . . . ¼ 0;
E2 ¼ 46574206321997071371965342240537799600v33
pffiffiffi
 81167268348022878125317175046116942012v43 3v36  ½. . . 95 terms . . . ¼ 0.

The resultant of these two equations, by eliminating v3, yields a univariate polynomial in v6 of degree 48
which can be factored into a polynomial P1 of degree 16, a polynomial P22 of degree eight squared and in
8
ð1 þ v26 Þ . Because of the ‘‘nice’’ values for the example P1 factors in a polynomial of degree one, yielding
the solution for the joint parameter of the sixth joint of the set of joint angles we started with and a polynomial
of degree 15. The 16 roots of P1 are

v6 2 f0:2679491920; 0:1651548583; 0:1310929573  0:8370455908i; 0:05048057402

 1:050657450i; 0:02205333535  1:054814962i; 0:02709648159  0:9336471816i; 0:1015095637
 1:085505997i; 0:1313072143  0:2075318817i; 0:2310214712  0:2195257104ig.

Back substituting one value for v6 in the equations E1 and E2, solving for v3 and taking the common solution
every value of v6 yields exactly one value for v3. The 16 solutions for v3 are

v3 2 f0:5773502693; 0:5513075395; 0:7426315981  0:8394355258i; 0:4728081593

 0:08258900521i; 0:2116842781  0:8023896617i; 0:3899070234  4:124398357i; 0:3980795285
 0:2213303775i; 0:5648941737  2:233721302i; 0:6413657826  0:1234273129ig.

We can see here, that the first values (0.2679491920, 0.5773502693) in the two lists are exactly the values for
p
 tan 12 and  tan p6, corresponding to the given angles u6 ¼  p6 and u3 ¼  p3 (compare Eq. (A.1)).
We show the back substitution for this set of solutions. Substituting these two angles in the 3R-chains we
have two 2R-chains for which we know the coinciding poses of the end-effectors. We also know the matrices
that describe the motions of these two chains. These are exactly the same matrices as for the two 3R-chains
with the exception that the angles u3 and u6 are now fixed. We substitute the value of u3 in T1 and the
corresponding value of u6 in T2 to describe the motion of the 2R-chains.
2 1 3
t11 ðu1 ; u2 Þ    t114 ðu1 ; u2 Þ
6 .. .. 7
Te 1 ðu1 ; u2 Þ ¼ 6 7;
4 . . 5
t141 ðu1 ; u2 Þ    t144 ðu1 ; u2 Þ
2 3
t211 ðu4 ; u5 Þ    t214 ðu4 ; u5 Þ
6 .. .. 7
e 2 ðu4 ; u5 Þ ¼ 6
T 7.
4 . . 5
t241 ðu4 ; u5 Þ    t244 ðu4 ; u5 Þ

The pose of the end-effector of the two 2R-chains (R = RL = RR) is obtained by back substituting the solutions
of v3 and v6 in the solutions of the linear system of equations to determine the Study parameters of this pose.
We obtain the matrix:
2 3
1 0 0 0
6 153:7703938 0:7577722258 0:3788861134 0:5312499911 7
6 7
T ¼ ðtij Þ ¼ 6 7.
4 83:83813443 0:1874999986 0:9062500197 0:3788861144 5
83:92626995 0:6249999998 0:1875000013 0:7577722258
 M.L. Husty et al. / Mechanism and Machine Theory 42 (2007) 66–81 81

Table A.1
Sixteen solutions of the inverse kinematics
u1 u2 u3 u4 u5 u6
Solution 1 0.524 1.570 1.047 1.570 0.524 0.524
Solution 2 0.624 1.640 1.008 1.707 0.272 0.327
Solution 3 0.873 + 1.357i 0.477  1.715i 1.150 + 0.175i 0.710  1.220i 0.555 + 1.963i 0.749  2.175i
Solution 4 0.873  1.357i 0.477 + 1.715i 1.150  0.175i 0.710 + 1.220i 0.555  1.963i 0.749 + 2.175i
Solution 5 1.360  0.766i 1.010 + 1.942i 1.742  0.960i 0.093 + 3.057i 2.977 + 0.004i 2.383  3.356i
Solution 6 1.360 + 0.766i 1.010  1.942i 1.742 + 0.960i 0.093  3.057i 2.977  0.004i 2.383 + 3.356i
Solution 7 0.813 + 1.377i 3.041  3.846i 3.093 + 0.490i 1.969 + 4.311i 2.872  0.095i 2.770  3.549i
Solution 8 0.813  1.377i 3.041 + 3.846i 3.093  0.490i 1.969  4.311i 2.872 + 0.095i 2.770 + 3.549i
Solution 9 2.417  1.477i 2.394 + 4.353i 2.885  0.883i 2.709  4.845i 0.204 + 2.443i 0.402  3.295i
Solution 10 2.417 + 1.477i 2.394  4.353i 2.885 + 0.883i 2.709 + 4.845i 0.204  2.443i 0.402 + 3.295i
Solution 11 1.390 + 1.849i 2.467  2.308i 0.937 + 1.835i 2.699  2.228i 0.268 + 2.647i 2.319  2.756i
Solution 12 1.390  1.849i 2.467 + 2.308i 0.937  1.835i 2.699 + 2.228i 0.268  2.647i 2.319 + 2.756i
Solution 13 1.289  0.467i 2.211 + 0.523i 0.888  0.135i 1.929  0.612i 0.145 + 1.032i 0.273  0.414i
Solution 14 1.289 + 0.467i 2.211  0.523i 0.888 + 0.135i 1.929 + 0.612i 0.145  1.032i 0.273 + 0.414i
Solution 15 0.885  1.418i 0.873 + 1.359i 0.788 + 0.384i 1.993  1.438i 0.563 + 2.088i 0.475  0.422i
Solution 16 0.885 + 1.418i 0.873  1.359i 0.788  0.384i 1.993 + 1.438i 0.563  2.088i 0.475 + 0.422i

Now we compare entries of the matrices T e 1 and T

e 2 with the entries of the matrix T. This gives 12 equations for
the angles u1 and u2, resp. u4 and u5 and the common solutions of all equations yield, for every pair of values of
v3 and v6, exactly one value for each angle u1, u2, u4 and u5. The equations
t124 ¼ t24 ; t123 ¼ t23 ; t122 ¼ t22 ;
t131 ¼ t31
   
have the solution u1 ¼ 0:5235988065   p6 , u2 ¼ 1:570796143  p2 in common. The equations
t244 ¼ t4;4 ; t243 ¼ t4;3 ;
t234 ¼ t3;4 ; t233 ¼ t3;3
   
have the solution u4 ¼ 1:570796143  p2 , u5 ¼ 0:5235988065  p6 in common. The complete set of all 16
solutions is listed in Table A.1. The angles are given in radians and only three digits are displayed.

References

[1] H. Lee, C. Liang, Displacement analysis of the general 7-link 7RR mechanism, Mechanism and Machine Theory 23 (3) (1988) 219–
226.
[2] M. Raghavan, B. Roth, Inverse kinematics of the general 6R manipulator and related linkages, Transactions of the ASME, Journal of
Mechanical Design 115 (1990) 228–235.
[3] M. Ghazvini, Reducing the inverse kinematics of manipulators to the solution of a generalized eigenproblem, in: J. Angeles, G.
Hommel, Kovács (Eds.), Computational Kinematics, Solid Mechanics and its Applications, vol. 28, Kluwer Academic Publishers,
1993, pp. 15–26.
[4] J. Angeles, Fundamentals of Robotic Mechanical Systems. Theory, Methods and Algorithms, Springer, New York, 1997.
[5] M. Husty, A. Karger, H. Sachs, W. Steinhilper, Kinematik und Robotik, Springer-Verlag, Berlin, Heidelberg, New York, 1997.
[6] J. McCarthy, Geometric Design of Linkages, Interdisciplinary Applied Mathematics, vol. 320, Springer-Verlag, New York, 2000.
[7] J.M. Selig, Geometric Fundamentals of Robotics, Monographs in Computer Science, Springer, New York, 2005.
[8] E. Study, Geometrie der Dynamen, B.G. Teubner, Leipzig, 1903.
[9] K. Brunnthaler, H.-P. Schröcker, M.L. Husty, A new method for the synthesis of Bennet mechanisms, in: Proceedings of CK 2005,
Cassino, Italy, 2005.
[10] J. Naas, H. Schmid, Mathematisches Wörterbuch, vol. Band II, Akademie-Verlag, Berlin, 1974.
[11] J. Harris, Algebraic Geometry: A First Course, Graduate Texts in Mathematics, vol. 133, Springer, 1995.