Empirical
Empirical
Empirical
Roy Featherstone
Department of Systems Engineering
Australian National University
Canberra ACT 0200, Australia
March 6, 2004
Abstract
The joint-space inertia matrix of a robot mechanism can be highly ill-conditioned.
This phenomenon is not merely a numerical artifact: it is symptomatic of an under-
lying property of the mechanism itself that can make it more difficult to simulate or
control. This paper investigates the problem by means of an empirical study of the
eigenvalues, eigenvectors and condition number of the joint-space inertia matrix. It
is shown that the condition number is typically large, and that it grows anywhere
from O(N ) to O(N 4 ) with the number of bodies in the system. Several graphs are
presented showing how the condition number varies with configuration, the num-
ber of links, variations in link sizes, variations in connectivity, and fixed or floating
bases. Explanations are offered for some of the observed effects.
1 Introduction
The joint-space inertia matrix (JSIM) plays an important role in the simulation and
control of robot mechanisms. It is well known that the JSIM is both symmetric and
positive-definite, but it can also be highly ill-conditioned. This property is not merely a
numerical artifact: it is symptomatic of an underlying ill-conditioning phenomenon in the
mechanism itself. Thus, an ill-conditioned JSIM is a useful predictor of a loss of accuracy
in forward and inverse dynamics calculations, even if those calculations do not involve
the JSIM directly, and it suggests that the mechanism itself may be intrinsically more
difficult to control.
This paper investigates the nature and magnitude of the ill-conditioning problem by
means of a series of numerical experiments performed on a representative sample of robot
mechanisms. These experiments show how the condition number varies as a function of:
the number of links in the mechanism, the relative sizes of the links, the connectivity
(characterized by a branching-factor parameter), and a fixed or a floating base. The
0
This paper has been accepted for publication in the International Journal of Robotics Research, and
the final (edited, revised and typeset) version of this paper will be published in the International Journal
of Robotics Research, vol. 23, no. 9, pp. 859–871, September 2004 by Sage Publications Ltd. All rights
reserved. c Sage Publications Ltd.
1
experiments were conducted on planar, spherical, ‘circular’ and spatial mechanisms, and
in a variety of configurations. However, the investigation was limited to kinematic trees
containing only revolute joints. The experiments were performed using Matlab.
The main result of this investigation is that the condition number is indeed rather
large, even for quite innocuous mechanisms, and that it grows asymptotically with the
number of links anywhere from O(N ) to O(N 4 ). For example, a planar mechanism with
identical links (as defined in Section 3) has a maximum condition number of approximately
4 N 4 . If there are 40 links in the mechanism then the condition number can be as high as
107 , which would make the JSIM numerically singular in 32-bit floating-point arithmetic.
If the links are not all the same size then the condition number can be substantially
higher still. On the other hand, branched connectivity and a floating base both tend to
reduce the condition number. Even so, the results in this paper suggest that efforts to
simulate and control robotic systems with a large number of bodies will be hampered by
ill-conditioning to a much greater extent than might be supposed from the relatively good
results we currently get with typical 6-DoF robots.
Much is already known about the properties of the JSIM, precisely because of its cen-
tral role in many dynamics and control algorithms, but there does not appear to have
been any previous attempt to quantify the ill-conditioning problem. Many properties of
the JSIM were investigated by Tourassis and Neuman in [Tourassis and Neuman 1985a,
Tourassis and Neuman 1985b], but not the condition number. Ghorbel, Srinivasan and
Spong [Ghorbel, Srinivasan and Spong 1998] investigate the boundedness of the eigenval-
ues of the JSIM, which is an essential prerequisite for the application of some control
techniques. They come up with some bounds on the largest and smallest eigenvalue
from which an upper limit to the condition number could be deduced. Angeles and Ma
[Angeles and Ma 1988] quote figures in the range 1275–1399 for the condition number
of the PUMA 600 over some example trajectories, and figures in the range 3054–11934
for the Stanford Arm. They remark that these figures are large enough to cause (sig-
nificant) round-off errors in calculations involving the JSIM. Ascher, Pai and Cloutier
[Ascher, Pai and Cloutier 1997] consider the impact of ill-conditioning on the efficiency
of the dynamics simulation process. They show that some dynamics algorithms cope
better than others, and they suggest a modification to the articulated-body algorithm to
improve its performance on ill-conditioned systems. They also mention several examples
of robot mechanisms where ill-conditioning can be expected, including systems with long
kinematic chains or large differences in link sizes. Featherstone [Featherstone 1999] plots
the accuracy of several recursive dynamics algorithms against N . This data shows that
some algorithms are much more accurate than others, but that they all lose accuracy as N
increases. Both the work of Featherstone and of Ascher et al. shows that the problem lies
with the mechanism itself, since the effect is felt even by algorithms that do not calculate
or use the JSIM at any stage.
This paper continues with a short review of the ill-conditioning problem, followed by
a description of the robot models used in the experiments, followed by the experiments
themselves.
2
2 Ill-Conditioning
Let A be a nonsingular matrix. The condition number of A, denoted κ(A), is a measure of
the relative distance from A to the set of singular matrices [Golub and Van Loan 1989].
(Actually, it is the reciprocal of the distance.) It therefore depends on the choice of
underlying matrix norm. I have chosen to use the 2-norm in this paper, in which case the
condition number is defined as
σmax (A)
κ(A) =
σmin (A)
where σmax (A) and σmin (A) are the largest and smallest singular values, respectively, of
the matrix A. If A happens to be symmetric and positive-definite, like the JSIM, then
the singular values are the same as the eigenvalues, and the condition number can be
given by the alternative definition
λmax (A)
κ(A) =
λmin (A)
where λmax (A) and λmin (A) are the largest and smallest eigenvalues of A. If κ(A) is
large then A is said to be ill-conditioned.
The condition number of A defines an upper limit to the loss of precision in compu-
tations involving A. Given a system described by the equation
y = Ax,
y + δy = A (x + δx) .
The larger the value of κ(A), the greater the potential for loss of accuracy.
The practical significance of ill-conditioning can be illustrated with a simple example.
Suppose we wish to command the planar robot shown in Figure 1 to accelerate from rest
with a desired joint acceleration vector of
where τ is the joint force vector, q and q̇ are the joint position and velocity vectors,
H(q) is the JSIM, and C(q, q̇) is the sum of the gravity, Coriolis and centrifugal terms.
3
y
However, as this robot moves in a horizontal plane (no gravity terms), and is initially at
rest (q̇ = 0), the term C(q, q̇) simplifies to zero in this particular instance. The exact
value of τ required to produce q̈d is therefore
302.0450 · · ·
250.2104 · · ·
τ d = H q̈d = .. .
.
8.6767 · · ·
If we apply exactly τ d to the robot then we will get the desired response; but what happens
if we apply a slightly different force? Suppose the actuators are slightly imperfect, so that
the actual applied force, τ a , differs very slightly from τ d . Let us say that τ a is τ d rounded
to three significant figures. The robot’s response to τ a will be
302 0.7917
250
1.0281
201 1.4904
151 0.6886
q̈a = H−1 τ a = H−1 = . (1)
106 1.1026
64.5
1.0911
31.5 0.5626
8.68 1.2384
The actual response is very different from the desired response, with errors of up to
50% on individual joint axes, and yet the applied forces were all accurate to better than
0.5%. High sensitivity to changes in inputs or parameters is a characteristic property of
ill-conditioned systems.
In this example, the relative error was magnified by a factor of about 100; but κ(H) '
1500 in the configuration shown, and there are other configurations where κ(H) > 10, 000,
so the mechanism is capable of exhibiting far greater sensitivity than was evident in this
one example.
The acceleration errors lie predominantly in a subspace of joint motions that cause
the least movement of the bodies in the robot mechanism. This raises the question
4
of whether this is indeed an important effect, or something that can be ignored. In a
simulation context, these are the fast modes of a stiff system, and therefore cannot be
ignored. In a control context, the answer is less clear. It probably depends on how the
control system works. Any control system that uses an inverse dynamics calculation to
linearize the plant is likely to be adversely affected.
Another point to note is that the problem lies with the robot itself, not just the JSIM.
Eq. 1 gives an accurate value for the acceleration response to an applied force of τ a ; and
the result would not be any different if the computation were performed in a manner
that bypassed the calculation and use of the JSIM (e.g. by using the articulated-body
algorithm). So it is fair to say that the mechanism itself is ill-conditioned in some sense,
and that the condition number of the JSIM is a manifestation of this physical property
rather than a purely numerical artifact.
Having put forward the idea that a rigid-body system can be ill-conditioned, it is
natural to ask what exactly this means, and how it can be quantified. Unfortunately,
there is no easy answer to these questions. The obvious way to proceed is to define the
condition number of a mechanism at a specified configuration to be the ratio of the largest
and smallest responses to an applied force of unit magnitude at that configuration. The
problem with this definition is that there is no natural metric on the space of generalized
forces, and likewise no natural metric on the space of generalized accelerations. Without
appropriate metrics, one cannot define what is meant by a unit force or the ratio of the
magnitudes of two accelerations.
It is possible to proceed by defining an artificial metric. Only one metric is required
because a metric defined on one space will induce a metric on the other. In the general
case, a mechanism can contain both revolute and prismatic joints; so an artificial metric
must answer the question of how many radians of rotation shall be considered equal in
magnitude to a displacement of one metre. I have chosen to avoid this issue by excluding
mechanisms containing non-revolute joints. The artificial metric used in this paper is a
Euclidean metric in a system of joint coordinates such that the unit acceleration is one
radian per second per second and the unit force is one Newton-metre. (One must specify
the units as the JSIM is not invariant w.r.t. a change of units.) The final experiment in
this paper compares this metric with an inertia-weighted metric.
The main difficulty with using an artificial metric is that the eigenvalues and eigenvec-
tors of the JSIM depend on the choice of metric. This is because the JSIM represents a
mapping from one vector space to another. The standard formula for defining eigenvalues
and eigenvectors is
Ax = λx, (2)
but this formula assumes that A is a mapping of a vector space into itself, i.e., A : U 7→ U
where U is a vector space. If, instead, A is a mapping from one vector space to another,
e.g. A : U 7→ V , then Eq. 2 cannot be used because the LHS is an element of V , the
RHS is an element of U , and the two cannot be equated. One must instead use the more
general formula
Ax = λBx (3)
where B is a mapping of the same kind as A. The problem with Eq. 3 is that λ and x are
now clearly functions of both A and B rather than A alone. If we use Eq. 3 to define the
eigenvalues and eigenvectors of the JSIM then the value of B is determined by the choice
5
joint 1 3 5 8 12
13
0 1 2 9
14
6
10 15
4
link 1 16
7 11
fixed base (=root) 17
of artificial metric. For the metric used in this paper, B is an identity matrix in joint
coordinates. This means we can use Eq. 2 for the numerical determination of eigenvalues
and eigenvectors.
3 Robot Models
The experiments reported in this paper were carried out using two parametric families of
robot mechanisms. For identification purposes, one is called hydra and the other is called
onion. Both consist of tree-structured rigid-body systems with revolute joints and a fixed
base. There are four parameters altogether, and they have the same meanings in both
families: N is the number of bodies, B is the branching factor, α is the angle between
successive joint axes, and ρ controls the relative sizes of the links. For convenience, we
use the following notation to refer to these robots: hydra(N, B, α, ρ) means the set of all
hydra robots; onion(N, 1, 0, 1) means the set of all onion robots in which B = 1, α = 0
and ρ = 1; hydra(33, 1, 0, 1) refers to a specific robot; and so on.
The connectivity of a hydra or onion mechanism is defined as follows. First, the fixed
base serves as the root of the kinematic tree, and is identified as link number zero. The
moving links are then numbered consecutively from 1 to N . The connectivity is described
by a body connection array, P , such that P (i) is the link number of the parent of link i.
And finally, the joints are numbered such that joint i connects from link P (i) to link i.
P is computed from the branching factor according to the formula
where the operators b· · ·c and d· · ·e round the enclosed expressions down and up, respec-
tively, to the nearest integer. This formula produces a kinematic tree in which the root
node has exactly one child, but the other nonterminal nodes have an average of B children
each. If B = 1 then P (i) = i − 1 and the result is an unbranched kinematic chain. If
B = 2 then P = [0, 1, 1, 2, 2, . . .] and the result is a binary tree. If B is not an integer
then the nonterminal nodes alternate between bBc and dBe children in the correct ratio
for the average to be B. For example, if B = 1.5 then one half of the nonterminals have
one child and the other half have two (see Figure 2). A tree constructed according to this
formula has minimum depth for the given parameters, but is not necessarily balanced.
The skew parameter, α, is the angle between a link’s inboard and outboard joint axes,
measured about the common perpendicular from the inboard axis to the outboard axis.
This angle is the same for every link in the mechanism. The inboard axis is the one used
6
z
inboard
axis
outboard
y axis
α
x =1
by the inboard joint, which is the joint that connects the link to its parent. The outboard
axis is used by the outboard joints, if any, which connect the link to its children. The base
link is a special case: it does not have an inboard axis, and its outboard axis coincides
with the z axis of base coordinates. If a link has more than one child then it has more
than one outboard joint, but they all use the same axis. If α = 0 then every axis in the
mechanism is parallel.
The tapering parameter, ρ, specifies a size ratio between consecutively-numbered links:
the length parameters of link i + 1 are those of link i multiplied by ρ; the mass of link
i + 1 is ρ3 times that of link i; and the rotational inertia of link i + 1 is ρ5 times that of
link i. These ratios are consistent with link i + 1 being scaled geometrically with respect
to link i while keeping the density constant. If ρ = 1 then every link in the mechanism is
identical.
hydra(N, B, α, ρ) is a family of spatial robots that simplify to planar robots when
α = 1. Link 1 of a hydra robot is modelled as a thin-walled cylindrical tube of length
1m and radius 0.05m (see Figure 3). The tube is centred on the x axis of the link’s local
coordinate frame, and it lies between (0,0,0) and (1,0,0) in local coordinates. The inboard
joint axis coincides with the local z axis; and Link 1’s x axis coincides with the x axis of
base coordinates when q1 = 0. The outboard joint axis passes through the point (1,0,0) in
the direction (0, − sin(α), cos(α)). The link has a mass of 1kg, a centre of mass at (0.5,0,0),
and a rotational inertia about its centre of mass of diag(0.0025, 1.015/12, 1.015/12). The
kinematic and inertia properties of the other links are those of Link 1 scaled according to
the tapering formulas.
onion(N, B, α, ρ) is a family of spherical robots: every joint axis passes through a
single point at the origin of base coordinates, so every link is constrained to rotate about
this one point. One possible implementation of an onion robot is shown in Figure 4. If
α = 0 then every joint axis is coincident with the z axis of base coordinates, and the links
are therefore constrained to rotate about this one axis. This kind of mechanism does not
appear to have an official name, so I will call them circular mechanisms, in analogy with
planar and spherical mechanisms. The links of an onion robot are spherically symmetric:
their centres of mass lie at the origin, and their rotational inertias are characterized by a
single scalar. Link 1 has a rotational inertia of 1kgm2 . It is also assigned a mass of 1kg,
but the link mass parameter is unimportant, as it does not affect the equation of motion
of this class of robots. The inertia parameters of the other links are calculated via the
tapering formulas above.
7
α z
y
These two families were designed to give a broad coverage of several classes of mech-
anism while keeping the number of parameters small. They allow us to examine spatial,
planar, spherical and circular mechanisms, branched and unbranched kinematic chains,
and systematic variations in link sizes and inertias.
4 Results
This section presents the results of a series of numerical experiments to investigate the
condition number, eigenvalues and eigenvectors of the JSIM of a large sample of hydra
and onion robot mechanisms. The experiments were performed using Matlab.
zag(θ) : qi = (−1)i+1 θ , i = 1 . . . N
and
curl(θ) : qi = θ , i = 1 . . . N .
Their names were chosen for their effect on planar hydra robots: zag(θ) puts them into
a zigzag shape, and curl(θ) winds them around a circle. Some examples are shown in
Figures 5 and 1. Two robots are in comparable configurations if they are both in zag(θ)
8
(a) (b)
y
curl(0.8)
x zag(0.6)
zag(2.4)
zag(3.0)
(c)
(d) (e)
configurations with the same value of θ, or are both in curl(θ) configurations with the
same value of θ. Note that zag(0) = curl(0) and zag(π) = curl(π). Note also that none
of these configurations are singular from a dynamics point of view. A configuration like
zag(0) is a kinematic singularity for a hydra robot, but this singularity appears in the
Jacobian, not the equation of motion.
Two experiments were performed, and the results are shown in Figures 6 and 7. The
first experiment used zag(θ) configurations and the second used curl(θ). The curves
connect comparable configurations at different values of N . The sample set for N consists
of the integers 1 . . . 8 followed by values from the E12 series (10, 12, 15, 18, 22, . . . ) up
to 560.
Perhaps the first thing to observe is simply the magnitude of the condition number.
It’s already up to 44 for a 2-DoF robot, 306 for a 3-DoF robot, and over 40,000 for a
10-DoF robot. A matrix can be considered numerically singular if its condition number
is close to the resolution limit of the machine arithmetic. By this measure, the JSIM is
already potentially singular in single-precision IEEE floating-point arithmetic at N = 40.
(The corresponding figure for double-precision arithmetic is N ' 6000.)
One obvious feature of the curves is that their straight sections have only two slopes.
The steeper slope corresponds to a fourth-power relationship between condition number
and N , i.e., κ(H) = O(N 4 ), and the shallow slope corresponds to a second-power rela-
tionship, κ(H) = O(N 2 ). An approximate formula for the worst case is κmax (H) ' 4 N 4 .
In Figure 6 the shallow slope is only evident at values of θ approaching π, and there
is a fairly sharp transition from the shallow slope to the steep one at a value of N that
increases as θ gets closer to π. It turns out that the transition occurs when the zigzag
shape formed by the robot covers an area that is approximately square in outline. This
happens when N ' sec(θ/2). In the case of θ = 3.0, for example, the transition occurs
at N = 14. If N < 14, as in Figure 5(d), then the zigzag is wider than it is long; and if
N > 14, as in Figure 5(e), then the zigzag is longer than it is wide.
This suggests that the composite-rigid-body inertia1 of the robot as a whole is one of
1
The inertia that a specified collection of rigid bodies would have if they were rigidly connected
9
12
10
θ=0
0.6
10 1.6
10 2.4
2.8
8 3.0
10
3.1
6
10
π
4
10
2
10
0
10 0 1 2 3
10 10 10 10
12
10
θ=0
0.012
10
10 0.025
0.05
0.1
8 0.2
10
0.4
0.8
6
10 1.4
π
2.4
4
10
2
10
0
10 0 1 2 3
10 10 10 10
10
the factors in the relationship between κ(H) and N . At values of N below the transition,
the robot is gaining mass in linear proportion to N ; but its geometrical extent, and hence
also its radius of gyration, are dominated by the width of the zigzag, which is not varying
with N . At values of N above the transition, the geometrical extent is dominated by the
length of the zigzag, which grows linearly with N , so the radius of gyration also grows
approximately linearly with N . Thus, the rotational inertia of the whole mechanism grows
in proportion to N below the transition, and in proportion to N 3 above the transition.
This hypothesis is supported by the results in Figure 7. In this figure, the steep slope
is only evident at small values of θ, and the curves start out with the steep slope and
make a transition to the shallow slope. The transition is less sharp, and is followed by a
decaying undulation before the curve settles down to the shallow slope. What’s happening
here is that the mechanism curls into a circular arc of radius approximately 1/θ. As N
increases, the arc progresses around the perimeter of the circle, and becomes a complete
circle when N ' 2π/θ. The transition begins as the arc approaches a semicircle, and the
slope reaches its first minimum when the arc has traced slightly more than one complete
circle. Thus, at low values of N , the radius of gyration is growing approximately linearly
with N , but, at higher values, it reaches an upper limit determined by the radius of the
circle.
11
12
10
θ=0
α = 0.5 (3D)
10 1.6
10 α = 0 (planar)
2.8
8
10
3.1
6
10
π
4
10
2
10
0
10 0 1 2 3
10 10 10 10
parity in link sizes can produce an ill-conditioned JSIM [Ascher, Pai and Cloutier 1997],
but we will examine how link size ratios and N together affect the condition number.
The tapering parameter, ρ, allows us to construct robot mechanisms in which the link
sizes follow a geometric progression: the size of link i is that of link i − 1 scaled by ρ.
For any fixed value of ρ other than 1, the size ratio between the largest and smallest
links grows exponentially with N ; so we would expect the size-ratio effect to dominate
all others at sufficiently high values of N . This is borne out by the results in Figure 9.
It shows the condition number of hydra(N, 1, 0, ρ) plotted against N for various values
of ρ in configurations curl(0) and curl(π/2). The curves for ρ = 0.7 end at a size ratio
of approximately 600 : 1, and the curves for ρ = 0.99 end at ratios of 28 : 1 in curl(0)
and 280 : 1 in curl(π/2). The curves stop just before the JSIM goes singular (in double-
precision arithmetic).
A more interesting effect is shown in Figure 10. In this experiment, the condition num-
ber of hydra(N, 1, 0, ρ) was plotted against N in configurations of curl(0) and curl(π/2),
exactly as in Figure 9; but, instead of a fixed tapering coefficient, there is a fixed ratio
between the smallest and largest links in the mechanism. ρ is therefore a function of N ,
and it is given by the formula √
N −1
ρ= R
where R is the ratio of smallest to largest link size.
The curves are clearly exhibiting approximately the same asymptotic behaviour as
those in Figure 7. At N = 2, the condition number is dominated by the inertia ratio of
the two links, which is R5 . As N increases, the curves converge to some extent before
becoming approximately parallel. At N = 390, which is where the data ends, the ratios
between the condition numbers for R = 0.1 and R = 1 are 227.5 in curl(0) and 1587
12
16
10
0.99
14 θ=0 0.9
0.97
10 ρ = 0.7
θ = π/2
12
10 1.0
10
10
8
10
6
10 1.0
4
10
2
10
0
10 0 1 2 3
10 10 10 10
14
10
R= 0.1
θ=0
12 0.2
10 θ = π/2
0.4
1.0
10
10
8
0.1
10
0.2
6 0.4
10
1.0
4
10
2
10
0
10 0 1 2 3
10 10 10 10
13
10
10
8
10 N = 200
6
10
50
4 20
10
8
2
10
4
0
2
10
−2
10
0 0.2 0.4 0.6 0.8 1
Figure 11: Eigenvalues of hydra(N, 1, 0, 1) at zag(0), uniformly spread along the x axis,
for various N .
in curl(π/2). The ratios between R = 0.1 and R = 0.2 are 8.15 in curl(0) and 15.62 in
curl(π/2). These numbers are suspiciously close to the integers 8 and 16, but I have not
pursued this line of inquiry.
It would be nice if we could conclude that the results of Section 4.1 apply also to
robots with differing link sizes, provided the sizes vary within fixed limits. However, the
data presented so far is not sufficient to draw such a conclusion, since it relates only to
a geometric progression of decreasing link sizes. Other patterns of size variation would
have to be investigated.
14
10
10
8
10
N = 200
6
10
4 50
10
20
2
10
8
0 4
10
2
−2
10 −2 −1 0
10 10 10
Figure 12: Eigenvalues of hydra(N, 1, 0, 1) at zag(0) vs. normalized period, for various
N.
at zag(0) and a maximum of 0.164 at zag(2.45). For comparison, the smallest inertia
exhibited by a single link is Imin = 0.08458, which is its rotational inertia about its centre
of mass (in the x–y plane); so the smallest eigenvalue of the JSIM is actually smaller than
the smallest inertia of a single link by about a factor of 4.
This observation is not just an empirical result. In a few simple cases, it is possi-
ble to prove that the smallest eigenvalue is indeed bounded below by λmin ≥ Imin /4.
hydra(N, 1, 0, 1) at zag(0) is one such case; onion(N, 1, 0, 1) in any configuration is an-
other.
The curves in Figure 11 also show that most of the eigenvalues are small, and that
the larger eigenvalues constitute an ever-smaller proportion of the total as N increases.
For example, if we use the geometric mean of the smallest and largest eigenvalues as our
threshold, then the proportion of eigenvalues above this threshold dwindles from 50% at
N = 2 to 5% at N = 200.
In Figure 11, the curves appear to be converging everywhere except x = 1. This is
perhaps a little misleading, since the x scale is different for each curve. Figure 12 presents
a clearer picture of how the curves evolve. It shows the same data as Figure 11, but all
curves are now plotted on a single x scale such that the ith largest eigenvalue has an x
coordinate of 1/i. Thus, the largest eigenvalue appears at x = 1, the second largest at
x = 0.5, and so on. The result is a series of parallel, nearly-straight lines.
The justification for this scale is as follows. There is a close relationship between the
eigenvectors of the JSIM and the vibration modes of the robot. If the eigenvectors are
calculated using the Euclidean metric then they are the same as the vibration modes that
the robot mechanism would have if a unit-stiffness torsional spring were to be connected
across each joint. If the links are all in a straight line (as they are in zag(0)) then the
15
4
10
2
10
0
10 R=1
−2 0.4
10
0.2
−4
10
0.1
−6
10
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
16
0.6
0.5
0.4
0.3 e
3
0.2
0.1 e
d 1
1
0
−0.1
e
2
−0.2
−0.3
−0.4
0 2 4 6 8 10 12 14 16 18 20
eigenvalue decreases slightly faster than the composite inertia of the whole mechanism
(i.e., slightly faster than H11 ). Asymptotically, the former dominates. These results
provide part of the explanation for the effects seen in Figure 10.
Now let us look at some eigenvectors. Figure 14 shows the three largest eigenvectors
of hydra(20, 1, 0, 1) (e1 , e2 and e3 ) and the largest eigenvector of hydra(10, 1, 0, 1) (d1 ),
all plotted in joint coordinates (i.e., the ith circle on each curve is the joint-i coordinate of
the eigenvector). e2 and e3 have been included only to illustrate the connection between
eigenvalues and vibration modes.
Figure 14 provides us with an explanation of why, in Figures 6 and 7, the condition
number grows faster by one power of N than the composite inertia of the whole mech-
anism. Observe that the coordinates of d1 and e1 are all positive. This means that
the corresponding joint motions sum along the length of the mechanism. The effect is
illustrated in Figure 15, which shows hydra(10, 1, 0, 1) in the configuration 0.2 d1 and
17
hydra(20, 1, 0, 1) in 0.2 e1 , the two robots being scaled to appear the same size. Observe
that hydra(20, 1, 0, 1) has curved more than hydra(10, 1, 0, 1), despite the fact that both
vectors have the same Euclidean magnitude. This is because 20
P10
i=1 e1i > i=1 d1i even
P
P20 2 P10 2
though i=1 e1i = i=1 d1i = 1. In fact, one √ can show that the sum of the elements of
the largest eigenvector grow in proportion to N . √
The effect is like having a gear box with a gear ratio of N interposed between joint
space and the mechanism itself: if the actual inertia is growing in proportion to N x then
the apparent inertia in joint space is growing in proportion to N x+1 , being the actual
growth multiplied by the square of the gear ratio. As there is no gearing effect on the
smallest eigenvalue, the net effect on the condition number is that it too grows by one
power of N faster than the composite inertia of the mechanism.
For the mechanisms we have studied so far, the following formula accurately predicts
the asymptotic behaviour of κ(H), but not its magnitude:
Essentially, it says that the asymptotic behaviour is the product of the gearing effect and
the ratio of the largest and smallest inertias in the system. (It assumes that Link N is
the smallest link in the mechanism.)
18
6
10
B=1
5 1.025
10
1.1
4
1.333
10
2
3
10
2
10
1
10
0
10 0 1 2 3
10 10 10 10
Figure 16: Condition number of onion(N, B, 0, 1) vs. N at curl(0) for various values of
B.
0.35
B = 1.125
0.3
0.25
0.2
0.15 B=1
0.1
0.05
0
0 5 10 15 20 25 30 35
19
nine each lie on one of three subtrees, and the final eight each lie on one of four subtrees.
The individual joint coordinates are clearly weighted according to the number of links
supported by the joint: motion is concentrated into the joints that move the largest
number of links. The concentration becomes more pronounced as B increases.
Branching diminishes, and eventually eliminates, the gearing effect for two reasons.
The first is that the gearing effect depends on the summation of motion that occurs when
joints are connected in series. In an unbranched chain, all N joints are connected in series.
In a branched chain, the determining factor is the maximum number of joints between
any link and the base, which is, of course, given by the depth of the tree. With hydra and
onion robots, the depth of the tree grows in proportion to log(N ) when B > 1. The second
reason is that the gearing effect is largest when a large proportion of the joints contribute
almost equally to the motion, and is smallest when a small number of joints account for
most of the motion; so the gearing effect is reduced when the motion is concentrated into
the joints nearest the root.
20
12
10
θ=0
floating base 1.2
10
10 fixed base 0
1.2
8
10 3.05
6 3.05
10
π
π
4
10
2
10
0
10 0 1 2 3
10 10 10 10
Figure 18: Condition number of floating-base hydra(N +1, 1, 0, 1) compared with fixed-
base hydra(N, 1, 0, 1) in zag(θ) configurations.
21
To take an extreme example, consider floating-base hydra(3, 1, 0, 10) in zag(π/2). The
floating base in this mechanism is the smallest link by a factor of 10, and it determines
the magnitude of the smallest eigenvalue. The condition number of this mechanism is
105 ; but the condition number of its fixed-base equivalent is only 136, mainly because of
a large increase in the smallest eigenvalue.
q̈0 = D−1 q̈ , τ0 = Dτ
and
H0 = D H D ,
where
D = diag(d1 , d2 , . . . , dN )
(a diagonal matrix). If we choose di according to the formula
di = Hii−0.5
then the diagonal elements of H0 are all unity. Thus, D is the coordinate transformation
matrix that scales the units of measurement in joint space such that each joint sees a unit
inertia. The Euclidean metric in this scaled coordinate system is equivalent to an inertia-
weighted metric in the original coordinates; so, if we define κ0 (H) to be the condition
number of H according to the inertia-weighted metric, then
Figure 20 shows both κ(H) and κ0 (H) for hydra(N, 1, 0, 1) in zag(θ) configurations
for various values of θ. Overall, the two metrics produce very nearly the same results.
The inertia-weighted metric produces slightly smaller condition numbers at low values of
N and slightly larger condition numbers at large values of N .
This is an interesting result because it demonstrates that the well-known large differ-
ences in magnitude between the largest and smallest diagonal elements of H are not the
22
12
10
θ=0
scaled
10 1.6
10 unscaled
2.8
8
10
3.1
6
10
π
4
10
2
10
0
10 0 1 2 3
10 10 10 10
16
10
0.97
14 scaled 0.99
10
unscaled ρ = 0.9
12
10 1.0
10
10
8 0.99
10
6 0.97
10
4 0.9
10
2
10
0
10 0 1 2 3
10 10 10 10
23
cause of the ill-conditioning problem: if one scales the matrix to make all the diagonal
elements the same then the condition number goes up, not down, for large N .
Figure 21 shows κ(H) and κ0 (H) for hydra(N, 1, 0, ρ) in curl(0) for various values of ρ.
The two metrics are now clearly producing very different results: for ρ < 1, κ(H) grows
exponentially with N , while κ0 (H) converges to a limit that depends on ρ.
There is not enough data here to draw any firm conclusions, but it does suggest the
following physically-plausible hypothesis: The ill-conditioning of the underlying mechan-
ical system is caused largely by dynamic interactions between the bodies; the effect is
strongest when the bodies have similar inertias; and it diminishes to zero as the inertia
differences grow. This would imply that κ0 is providing a reasonable indication of the
underlying ill-conditioning when ρ < 1, while κ is providing an overestimate.
Additional support for this hypothesis comes from examining the elements of H0 . For
example, for hydra(5, 1, 0, ρ) in curl(0), we have
1 0.984 0.929 0.822 0.625
0.984 1 0.974 0.884 0.687
H0 =
0.929 0.974 1 0.952 0.769
0.822 0.884 0.952 1 0.883
0.625 0.687 0.769 0.883 1
when ρ = 1, and
1 0.594 0.252 0.097 0.034
0.594 1 0.593 0.251 0.092
H0 =
0.252 0.593 1 0.590 0.238
0.097 0.251 0.590 1 0.559
0.034 0.092 0.238 0.559 1
when ρ = 0.5. The off-diagonal elements Hij0 measure the dynamic interaction between
bodies i and j. These values are all lower when ρ = 0.5, and their magnitudes diminish as
the size difference between bodies i and j increases. H0 converges to the identity matrix
as ρ → 0.
5 Conclusion
This paper has presented an empirical study of the condition number of the joint-space
inertia matrix (JSIM), backed up with some investigation of the eigenvalues and eigen-
vectors of the JSIM. The study was restricted to two families of robots in which all the
joints were revolute; but it included examples of planar, spatial, spherical and ‘circular’
robots, systematic variations in relative link sizes, branched and unbranched chains, and
both fixed and floating bases. Condition numbers were calculated using two families of
comparable configurations: one in which all the joint angles were the same, and one in
which they alternated in sign.
The general conclusion one can draw from this study is that the condition number
can be very large, and it can grow asymptotically with the fourth power of the number of
bodies in the system. For a simple planar chain of identical links, the maximum condition
number is approximately 4 N 4 , where N is the number of bodies. Thus, a chain of only
10 links already can exhibit a condition number of 40,000.
24
If the link sizes vary, then the condition number of the JSIM goes up as the size
differences increase; but if the JSIM is first scaled to have unit elements along the diagonal,
then the condition number of this scaled matrix goes down as the size differences increase.
Branches in the kinematic chain can reduce the growth in the condition number by up to
one power of N compared with an unbranched chain. A floating base can make matters
worse, but typically yields a modest reduction in condition number that does not appear
to grow with N .
The significance of these results is that ill-conditioned systems are both harder to
simulate accurately and harder to control. Although a typical 6-DoF robot arm can be
simulated and controlled perfectly adequately, this does not mean that the same would
necessarily be true of a larger system, since the condition number grows so rapidly with
N . This is not just a problem with the JSIM: it is a physical property of the robot
mechanism, and will therefore have an effect even on simulators and control systems that
do not use the JSIM.
And finally, all the results in this paper depend on an arbitrary choice of metric. Only
two metrics were investigated: a Euclidean metric in a configuration space of revolute
joint angles, and an inertia-weighted version of the Euclidean metric. A different choice
of metric may well produce different results.
References
[Angeles and Ma 1988]
Angeles, J. and Ma, O. 1988. Dynamic Simulation of n-Axis Serial Robotic Manip-
ulators Using a Natural Orthogonal Complement. Int. J. Robotics Research, vol. 7,
no. 5, pp. 32–47.
[Ascher, Pai and Cloutier 1997]
Ascher, U. M., Pai, D. K., and Cloutier, B. P. 1997. Forward Dynamics, Elimination
Methods, and Formulation Stiffness in Robot Simulation. Int. J. Robotics Research,
vol. 16, no. 6, pp. 749–758.
[Featherstone 1987]
Featherstone, R. 1987. Robot Dynamics Algorithms. Boston: Kluwer Academic Pub-
lishers.
[Featherstone 1999]
Featherstone, R. 1999. A Divide-and-Conquer Articulated-Body Algorithm for Par-
allel O(log(n)) Calculation of Rigid-Body Dynamics. Part 2: Trees, Loops and Ac-
curacy. Int. J. Robotics Research, vol. 18, no. 9, pp. 876–892.
[Ghorbel, Srinivasan and Spong 1998]
Ghorbel, F., Srinivasan, B., and Spong, M. W. 1998. On the Uniform Boundedness
of the Inertia Matrix of Serial Robot Manipulators. J. Robotic Systems, vol. 15, no.
1, pp. 17–28.
[Golub and Van Loan 1989]
Golub, G. H., and Van Loan, C. F. 1989. Matrix Computations. Baltimore: The
Johns Hopkins University Press.
25
[Tourassis and Neuman 1985a]
Tourassis, V. D., and Neuman, C. P. 1985. Properties and Structure of Dynamic
Robot Models for Control Engineering Applications. Mechanism & Machine Theory,
vol. 20, no. 1, pp. 27–40.
26