You are on page 1of 10

IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO.

5, OCTOBER 2010 827

A Motion-Planning Algorithm for the


Rolling-Body Problem
François Alouges, Yacine Chitour, and Ruixing Long

Abstract—In this paper, we consider the control system Σ de- element of S2 , and finally, one more parameter for the relative
fined by the rolling of a strictly convex surface S of IR3 on a orientation of S1 with respect to S2 [17], [18]. Therefore, the
plane without slipping or spinning. The purpose of this paper is to state space Q(S1 , S2 ) of Σ is a 5-D manifold, and it can be
present the numerical implementation of a constructive planning
algorithm for Σ, which is based on a continuation method. The per- shown that Q(S1 , S2 ) is a circle bundle over S1 × S2 . Due to
formances of that algorithm, both in robustness and convergence the rolling constraints (i.e., no slipping and no spinning), one
speed, are illustrated through several examples. easily shows that once an absolutely continuous (a.c.) curve c1
Index Terms—Manipulation planning, nonholonomic motion on S1 is prescribed, there exists a unique a.c. curve Γ in Q, which
planning. describes the rolling without slipping or spinning of the surface
S2 onto the surface S1 along the curve c1 . Thus, the admissible
I. INTRODUCTION inputs of the control system Σ exactly correspond to the a.c.
N recent years, nonholonomic systems have attracted much curves c1 of S1 by their velocities ċ1 . As a consequence, Σ can
I attention due to the theoretical questions raised for their
motion planning and due to their importance in numerous appli-
be written (in local coordinates) as a driftless control system of
the type ẋ = u1 F1 (x) + u2 F2 (x), where (u1 , u2 ) ∈ IR2 is the
cations (see [15] and [16] and references therein). In particular, control, and F1 and F2 are vector fields defined in the domain
the planning of robotic manipulators to achieve high operational of the chart (see [15] and [16] and references therein). With re-
capability with low constructive complexity is a major issue for gard to controllability issues, there exist several works (see [15]
the control community in the past decade. Nonholonomy is ex- and references therein) addressing these questions. Agrachev
ploited for the design of such manipulators but ensuring both and Sachkov [1] proved that Σ is completely controllable if and
hardware reduction and controllability performances yields se- only if S1 and S2 are not isometric. Marigo and Bicchi [15]
rious difficulties, thereby requiring more elaborate analysis and provided geometric descriptions for the possible reachable sets.
an efficient algorithm. The rolling-body problem illustrates well One of the main conclusions of these works will be instrumental
all the aforementioned aspects. for us and goes as follows: The control system Σ is locally con-
We recall that the rolling-body problem (i.e., without slipping trollable at a point q ∈ Q if KS 1 (pr1 (q)) − KS 2 (pr2 (q)) = 0,
or spinning) is a control system Σ, which models the rolling of a where KS 1 (·) and KS 2 (·), respectively, denote the Gaussian
connected surface S1 on another one S2 of the Euclidean space curvature of S1 and S2 , and pri : Q → Si , i = 1 and 2 are the
IR3 so that the relative speed of the contact point is zero (i.e., canonical projections. In particular, if S1 is a strictly convex sur-
no slipping) and that the relative angular velocity has zero com- face (i.e., KS 1 (q1 ) > 0 for all q1 ∈ S1 ) and S2 = IR2 , then the
ponent along the common normal direction at the contact point control system (i.e., Σ) is not only completely controllable, but
(i.e., no spinning). It is intuitively clear that five parameters are also locally controllable at every point q ∈ Q. On the contrary,
needed to describe the state of Σ: two to parameterize the con- the following result is worth mentioning: Σ is not completely
tact point as element of S1 , two others for the contact point as controllable if and only if S1 and S2 are isometric, with an
isometry of IR3 .
Regarding the motion-planning problem (MPP) associated
with the rolling-body problem, most of the attention focused
Manuscript received; revised March 6, 2010; accepted June 15, 2010. Date
of publication July 26, 2010; date of current version September 27, 2010. This
on the rolling of a convex surface S on a flat one, due to the
paper was recommended for publication by Associate Editor F. Lamiraux and fact that the latter models dexterous robotic manipulation of
Editor J.-P. Laumond upon evaluation of the reviewers’ comments. The work a convex object by means of a robotic hand with as few as
of F. Alouges was supported by the Chair Mathematical modeling and numer-
ical simulation, F-EADS Ecole Polytechnique Institut National de Recherche
three motors and flat finger (see [15] and [16] and references
en Informatique et en Automatique (INRIA) F-X È. The work of R. Long was therein). Moreover, in [15], several prototype dexterous grippers
supported by the grants from DIGITEO and Région l’Ile de France. This pa- are exhibited. Recall that the MPP is the problem of finding a
per was presented in part at the IEEE Conference on Decision and Control,
Shanghai, China, 2009.
procedure that, for every pair (p, q) of the state space of a control
F. Alouges and R. Long are with the Department of Applied Mathemat- system Σ, effectively produces a control up,q giving rise to an
ics, Ecole Polytechnique, Palaiseau 91128, France (e-mail: francois.alouges@ admissible trajectory steering p to q. Note that in the category
cmap.polytechnique.fr; ruixing.long@polytechnique.edu).
Y. Chitour is with the Laboratiore des Signaux et Systèmes, Uni-
of rolling-body problem, even the simplest model, which is
versité Paris-Sud XI, Center National de la Recherche Scientifique the so-called plate–ball system (i.e., a sphere rolling on the
and École Supérieure d’Électricité, Gif-sur-Yvette 91192, France (e-mail: plane), allows no chained-form transformation and is not a flat
yacine.chitour@lss.supelec.fr).
Color versions of one or more of the figures in this paper are available online
system. We can hierarchize this category of problems as follows
at http://ieeexplore.ieee.org. according to increasing level of difficulty.
Digital Object Identifier 10.1109/TRO.2010.2053733

1552-3098/$26.00 © 2010 IEEE


828 IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 5, OCTOBER 2010

L1: It includes S1 rolling on the plane. amounts to first showing that P (Π(s)) exists (for instance, if
L1-1: Here, the plane is free of prohibited regions. DF (Π(s)) is surjective) and, second, to prove that the ODE in
L1-2: There are prohibited regions (i.e., obstacles) on the X, Π̇(s) = P (Π(s))π̇(s), which is a “highly” nonlinear equa-
plane. tion (which is also called the path-lifting equation (PLE) or
L1-3: There are prohibited regions on S1 . Wazewski equation [24]), admits a global solution. In the con-
L2: It includes S1 rolling on the top of S2 with S2 nonflat, and text of the MPP, the CM was introduced in [11], [20], and [21]
S1 and S2 nonisometric. and further developed in [8]–[10], [22], and [23]. The map F is
now an endpoint map from the space of admissible inputs to the
L2-1: There are no prohibited regions neither on S1 nor on
state space. Its singularities are exactly the abnormal extremals
S2 .
of the sub-Riemannian metric induced by the dynamics of the
L2-2: There are prohibited regions on S1 or (and) S2 .
system, which are usually a major obstacle for the CM to apply
For L1, there exists essentially one family of methods, which efficiently to the MPP. In the case of Σ, nontrivial abnormal
is commonly called geometric-phase methods, based on the extremals and their trajectories were determined in [7], and they
Gauss–Bonnet theorem in differential geometry and initiated by exactly correspond to the horizontal geodesics of Σ. Despite
Li and Canny [14], which is a first general framework to solve that obstacle, assuming that the surface S is strictly convex and
L1-1. They devised an ingenious algorithm that solves the MPP possesses a stable periodic geodesic, it was shown in [7] that the
of the plate–ball problem efficiently. However, their method CM provides complete answers to the MPP. More precisely, it
cannot be directly applied to more-general convex surfaces S1 was shown that there exist enough paths π in the state space of
since explicit computation of the integral of the Gaussian curva- Σ that can be lifted to paths Π in the control space by showing
ture over a bounded region on S is, in general, not available. In global existence of solutions to the Wazewski equation.
the spirit of Li and Canny [14], Bicchi and Marigo [4] proposed In this paper, we provide full details for the numerical im-
an approximate motion-planning algorithm to solve L1-1 and plementation of the CM presented above in order to solve L1-1
L1-2 for general convex body S1 . By using a lattice structure efficiently. This paper is organized as follows. In Section II, we
on the state space, they translated Li–Canny’s global and exact present the kinematic equations of motion of a convex body S1
computation into a series of local and approximate ones (basic rolling without slipping or spinning on top of another one S2 . In
actions), which is easier to treat in practice. They also showed Section III, we describe how the CM can be applied to the MPP.
that this approximate method has good topological properties so Sufficient conditions guaranteeing the existence of P (Π) and
that it can be incorporated into a more-general motion-planning the existence of a global solution of the PLE in the case of the
algorithm dealing with obstacles in the plane. However, since rolling-body problem are also reported. Section IV details some
a fine-grid lattice is needed in order to improve the preci- key points for numerical resolution of PLE. In Section V, several
sion, a large number of periodical maneuvers is necessary to numerical simulations are presented. In Section VI, some de-
achieve the preassigned change of orientation, thus producing tailed comments and possible generalizations will be presented
highly oscillating-type motions, which may not be desirable in at the end of this paper.
practice.
Two other approaches to solve L1-1 were proposed in [7]. II. DESCRIPTION OF THE ROLLING-BODY PROBLEM
The first one is based on the Liouvillian character of Σ. One
In this section, we briefly recall how to derive the equations
can show that, if S1 admits a symmetry of revolution, then
of motion for the rolling-body problem with no slipping or
the MPP can be reduced to a purely inverse algebraic prob-
spinning of a connected surface S1 of the Euclidean space IR3
lem. However, such an approach presents a serious numerical
on top of another one S2 . This section does not bring new results;
drawback: The resulting inverse problem requires that implicit
however, we provide these results for the sake of completeness
functions must be determined through transcendental equations
as well as to exhibit the numerical challenges raised by trying to
involving local charts for S1 . The second approach proposed
implement ordinary differential equations on a manifold. These
in [7] is based on the well-known continuation method (CM,
results were already obtained, for example, in [1], [5], [6], [15],
which is also called homotopy method or continuous Newton’s
[16], and [18].
algorithm [2]), which dates back to Poincaré. The MPP is there-
We start by the intrinsic formulation of the problem, i.e.,
fore addressed as a pure inverse problem. Let us briefly recall
we first assume that S1 and S2 are 2-D, connected, oriented,
how the CM works. It is used to solve nonlinear equations of
smooth, complete Riemannian manifolds.
the form F (x) = y, where x is the unknown, and F : X → Y
is surjective. Let us consider x0 ∈ X, and y0 = F (x0 ). Let
A. Differential Geometric Notions and Definitions
us pick a differentiable path π : [0, 1] → Y joining y0 to the
given y. Then, the CM is an iterative procedure that lifts π If P is a matrix, we use P T and tr(P ) to denote, respectively,
to a path Π : [0, 1] → X so that F ◦ Π = π. The word “itera- the transpose of P and its trace.
tive” refers to the fact that the path Π is obtained by the flow Let (S, ·, ·) be a 2-D, connected, oriented, smooth, complete
of a differential equation defined on X. Indeed, one starts by Riemannian manifold for the Riemannian metric ·, ·. We use
differentiating F (Π(s)) = π(s) to get DF (Π(s))Π̇(s) = π̇(s). TS to denote the tangent bundle over S and US the unit tangent
The latter is satisfied by setting Π̇(s) := P (Π(s))π̇(s), where bundle, i.e., the subset of TS of points (x, v) such that x ∈ S
P (x) is a right inverse of DF (x). Therefore, solving F (x) = y and v ∈ Tx S, v, v = 1.
ALOUGES et al.: MOTION-PLANNING ALGORITHM FOR THE ROLLING-BODY PROBLEM 829

Let {Uα , α}α ∈A be an atlas on S. For α, β ∈ A such that Uα ∩ Given an a.c. curve c1 : [0, T ] → S1 , the rolling of S2 on S1
Uβ is not empty, we denote by Jβ α the Jacobian matrix of ϕβ ◦ without slipping or spinning along c1 is characterized by a curve
(ϕα )−1 the coordinate transformation on ϕα (Uα ∩ Uβ ). For Γ = (c1 , c2 , R) : [0, T ] → Q(S1 , S2 ), which is defined by the
α ∈ A, the Riemannian metric is represented √ by the symmetric following two conditions.
positive-definite matrix I α and set M α := I α . Up to initial conditions, the no-slipping condition amounts to
For x ∈ S, a frame f at x is an ordered basis for Tx S and, for
M α 2 ċα2 2 (t) = RM α 1 ċα1 1 (t) (2)
α, β ∈ A, we have f β = Jβ α f α . The frame f is orthonormal
if, in addition, M α f α is an orthogonal matrix. An orthonormal and the no-spinning one to
moving frame (OMF), which is defined on an open subset U
of S, is a smooth map assigning to each x ∈ U a positively ṘR−1 = Rω1 (ċα1 1 )R−1 − ω2 (ċα2 2 ). (3)
oriented orthonormal frame f (x) of Tx S. Since SO(2) is commutative, (3) reduces to
Let ∇ be the Riemannain connection on S (see, for ex-
ample, [19]). For a given OMF f defined on U ⊂ S, the ṘR−1 = ω1 (ċα1 1 ) − ω2 (ċα2 2 ). (4)
Christoffel symbols
 associated with f = (f1 , f2 ) are defined
If we fix a point x = (x1 , x2 , R0 ) ∈ Q(S1 , S2 ), a curve c1
by ∇f i fj = k Γkij fk where 1 ≤ i, j, k ≤ 2. The connection
on S1 starting at x1 defines entirely the curve Γ by (2) and (4).
form ω is the mapping defined on U such that, for every x ∈ U ,
Therefore, we can give the following definition.
ωx is the linear application from Tx S to the set of 2 × 2 skew-
Definition 1: The surface S2 rolls on the surface S1 without
symmetric matrices given as follows. For i, j, k = 1 and 2, the
slipping or spinning if, for every x = (x1 , x2 , R0 ) ∈ Q(S1 , S2 )
(i, j)th coefficient of ωx (fk ) is equal to Γkij .
and a.c. curve c1 : [0, T ] → S1 starting at x1 , there exists an a.c.
Let c : J → S be an a.c. curve in S with J compact interval of
curve Γ : [0, T ] → Q(S1 , S2 ), with Γ(t) = (c1 (t), c2 (t), R(t)),
IR. Set X(t) := ċ(t) in J, which defines a vector field along c.
Γ(0) = x and for every t ∈ [0, T ], such that, on appropriate
Let Y : J → TS be an a.c. assignment such that, for every t ∈ J,
charts, (2) and (4) are satisfied. We call the curve Γ(t) an ad-
Y (t) ∈ Tc(t) S. We say that Y is parallel along c if ∇X Y = 0
missible trajectory.
for almost all t ∈ J. In the domain of an OMF f , that equation
If we consider f1 and f2 be two OMFs and if the state x is
can be written as follows: Ẏ k = − 1≤i,j ≤2 Γkij X i Y j or equiv-
represented (in coordinates) by the triple x = (c1 , c2 , R), then
alently Ẏ = −ω(X)Y. Recall that a curve c is a geodesic if the for almost all t such that x(t) remains in the domain of an
velocity ċ(t) is parallel along c, i.e., appropriate chart, there exists a measurable function u(·) (which
∇ċ ċ = 0. (1) is called control) with values in IR2 such that
c˙1 (t) = u1 (t)f11 (c1 (t)) + u2 (t)f21 (c1 (t))
B. Rolling-Body Problem
c˙2 (t) = u1 (t)(f 2 (c2 (t))R(t))1
1) Definition of the State Space: Let us consider now the
rolling-body problem with no slipping or spinning of S1 on top + u2 (t)(f 2 (c2 (t))R(t))2
of S2 . We adopt here the viewpoint presented in [1]. 
2
At the contact points of the bodies x1 ∈ S1 and x2 ∈ S2 , Ṙ(t)R−1 (t)= ui (t)[ω1 (fi1 (c1 (t))) − ω2 (f 2 (c2 (t))R(t))i ].
their tangent spaces are identified by an orientation-preserving i=1
isometry q : Tx 1 S1 −→ Tx 2 S2 . Such an isometry q is a state of Let us consider the vector fields F1 and F2 defined by
the system, and the state space is given by
Fi = (fi1 , (f 2 R)i , [ω1 (fi1 ) − ω2 (f 2 R)i ])T , i = 1 and 2.
Q(S1 , S2 )
Then, the previous system of equations have the following com-
= {q : Tx 1 S1 → Tx 2 S2 |x1 ∈ S1 , x2 ∈ S2 , q isometry}.
pact form in local coordinates:
As the set of all orientation-preserving isometries in IR2 is
ẋ = u1 F1 (x) + u2 F2 (x). (5)
SO(2), which can be identified with the unit circle S 1 in IR2 ,
Q(S1 , S2 ) is a 5-D connected manifold. A point q ∈ Q(S1 , S2 ) We recognize the classical form of a driftless control-affine
is locally parameterized by (x1 , x2 , R) with x1 ∈ S1 , x2 ∈ S2 , system.
and R ∈ SO(2). Remark 1: In general, it is not possible to get a global basis
2) Rolling Dynamics: We next describe the motion of one for the distribution ∆ and, thus, to define globally the dynamics
body rolling on top of another one so that the contact point of of the control system using vector fields. One notable exception
the first follows a prescribed a.c. curve on the second body. occurs when one of the manifolds is a plane [7]. Therefore, ad-
Let f1 and f2 be two OMFs defined on the chart domains of dressing the motion planning efficiently (i.e., as far as producing
α1 , α2 . For i = 1 and 2, consider a curve cαi i defined inside the a numerical scheme) becomes a delicate issue since most of the
chart domain αi on the body Si . Let bi (t) = fi (ci (t))Ri (t) par- standard techniques are based on global vector-field expressions
allel along cαi i , i = 1 and 2, and R := R2 (t)R1 (t)−1 ∈ SO(2), of the dynamics of a control system.
which, by definition, measures the relative position of f2 with The following proposition describes a fundamental property
respect to f1 along (cα1 1 , cα2 2 ). The variation of Ri along cαi i , of the rolling-body problem; for more details, see, for instance,
for i = 1 and 2, is given by Ṙi = −ωi (ċαi i )Ri . [7].
830 IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 5, OCTOBER 2010

Proposition 1: Let u ∈ H be an admissible control that gives


rise to the admissible trajectory Γ = (c1 , c2 , R) : [0, 1] → M.
Then, the following statements are equivalent:
1) the curve c1 : [0, 1] → S1 is a geodesic;
2) the curve c2 : [0, 1] → S2 is a geodesic;
3) the curve Γ : [0, 1] → M is a horizontal geodesic.
Remark 2: In the case where S2 is a plane, if S1 is rolling along
a piecewise-linear curve c2 defined on S2 , then Proposition 1
allows us to construct the locus of the contact point on S1 .
Indeed, since S2 is flat, c2 is piecewise geodesic, then it suffices
to integrate a geodesic equation on S1 to get the locus of the
contact point; for more details, see Section IV-C.
3) Rolling-Body Problem in IR3 : From now on, we will as-
sume that the manifolds S1 and S2 are oriented surfaces of IR3
with metrics induced by the Euclidean metric of IR3 .
We first note that there are two possible ways to define
the rolling-body problem depending on the respective (global) Fig. 1. Continuation method.
choice of normal vectors for S1 and S2 . Indeed, the orientation
of the tangents planes of an oriented surface S is determined with
by the choice of a Gauss map, i.e., a continuous normal vec-  T
tor n : S → S 2 , with S 2 denoting the sphere of radius 1 in F1 (x) = 1, 0, cos ψ, −
sin ψ B2v 2
,− sin ψ (7)
IR3 . There are two such normal vectors, i.e., n and −n. If S B2 B2
is (strictly) convex, then these two normal vectors are called  T
1 cos ψ B1v 1 B2v 2
inward and outward, respectively. F2 (x) = 0, , − sin ψ, − ,− − cos ψ .
Recall that the rolling-body problem assumes that the tan- B1 B2 B1 B2
gent spaces at the contact points are identified. In IR3 , this is (8)
equivalent to identify the normal vectors. Let ni be the normal
vector of Si , then at contact points, we can either assign n1 to Remark 3: Since the functions B1 and B2 involving in the
n2 or −n2 , i.e., we have n1 = εn2 , with ε = ±1. The physical geodesic coordinates are obtained by solving partial differen-
meaning of this parameter ε is the following: If ε = 1, the two tial equations, the rolling dynamics given by (7) and (8) are
surfaces roll so that one is “inside” the other one; in other words, not completely explicit; thus, they may not be suitable for nu-
they are on the same side of their common tangent space at the merical implementations. We will explain in Section IV how to
contact point; if ε = −1, the two surfaces roll so that one is overcome this difficulty.
“outside” the other one; in other words, they are on opposite
sides with respect to their common tangent space at the contact III. CONTINUATION METHOD
point. It is clear that the second situation is more physically We start with a general description of the CM; for more details
feasible, in general, since it holds true globally as soon as the and complete justifications, see [8]. The state space Q(S1 , S2 ) is
two surfaces are convex. We will only deal with this second simply denoted by M . The admissible inputs u are elements of
situation. H = L2 ([0, 1], IR2 ). We use u(t) and u H , respectively, to
  1/2
We note that (5) has simpler expression in geodesic coor- 1
denote ( 2i=1 u2i (t))1/2 and 0 u(t) 2 dt . If u, v ∈ H,
dinates. Recall that the geodesic coordinates on a Riemannian 1 T
manifold S are charts (v, w) defined such that the matrix I α then (u, v)H = 0 u (t)v(t)dt.
is diagonal and equal to diag(1, B 2 (v, w)). The function B From the brief description of the CM given in Section I,
is defined in an open neighborhood of (0, 0) (i.e., the do- the map F is equal to the endpoint φp : H → M associated
main of the chart) and satisfies B(0, w) = 1, Bv (0, w) = 0, with some fixed p ∈ M . For u ∈ H and p ∈ M , let γp,u be the
and Bv v + KB = 0, where K denotes the Gaussian curvature trajectory of Σ starting at p for t = 0 and corresponding to u.
of S at (v, w), and Bv (Bv v , respectively) is the (double, re- Then, for v ∈ H, φp (v) is given by φp (v) := γp,v (1). Recall that
spectively) partial derivative of B with respect to v. φp (v) is defined for every v ∈ H. The MPP can be reformulated
Using the fact that Q(S1 , S2 ) is a circle bundle when S1 and as follows: For every p, q ∈ M , exhibit a control up,q ∈ H such
S2 are 2-D manifolds, and taking geodesic coordinates B1 , B2 that
for S1 and S2 at contact points x1 and x2 , respectively, consider
φp (up,q ) = q. (9)
coordinates x = (v1 , w1 , v2 , w2 , ψ) in some neighborhood of
(0, ψ0 ) in IR4 × S 1 . Then, the control system (5) can be written In other words, we want to inverse the endpoint map φp , or more
locally as precisely, we are looking for a right inverse of φp as this map is
surjective (by the controllability assumption) but not injective
(i.e., up,q is not unique). This inversion is performed by using
ẋ = u1 F1 (x) + u2 F2 (x) (6) the CM summarized in Fig. 1.
ALOUGES et al.: MOTION-PLANNING ALGORITHM FOR THE ROLLING-BODY PROBLEM 831

We start with an arbitrary control uinitial . Set q0 := Once the existence of a closed set K verifying the Condition 1
φp (uinitial ), and choose a path π : [0, 1] → M such that π(0) := is guaranteed, an application of Gronwall lemma yields that, for
q0 and π(1) := q. We now look for a path Π : [0, 1] → H such every path π : [0, 1] → K of class C 1 and every control ū ∈ H
that, for every s ∈ [0, 1] such that φp (ū) = π(0), the solution of the PLE defined in (12)
with initial condition ū exists globally on the interval [0, 1].
φp (Π(s)) = π(s). (10)
We now consider the MPP of a strictly convex surface S1
Differentiating (10) yields rolling on a plane. It is shown in [7] that if S1 verifies a simple
dΠ dπ geometric property, then there exists a compact subset K in
Dφp (Π(s)) · (s) = (s). (11) M verifying Condition 1, which is large enough to completely
ds ds
resolve the MPP. For a complete development on this issue,
If Dφp (Π(s)) has full rank, then (11) can be solved for Π(s) by see [7].
taking Π such that For numerical purposes, we recall here the structure of Sp
dΠ  dπ and φp (Sp ) for the rolling-body problem.
(s) = P Π(s) · (s) (12)
ds ds Proposition 2: For p ∈ M , we have
where P (v) is a right inverse of Dφp (v). For instance, we Sp = {(v cos θ, v sin θ)|v ∈ L2 ([0, 1], IR), θ ∈ [0, 2π]}
can choose P (v) to be the Moore–Penrose pseudoinverse of
Dφp (v). and φp (Sp ) is equal to the union of the endpoints of all horizontal
We are then led to study the Wazewski equation (12), which geodesics starting at p, i.e., all trajectories starting at p and
is called the PLE, as an ODE in H. Recall that, by construction, corresponding to one control u ∈ Sp .
the control defined by ufinal := Π(1) steers the system from p to In other words, Proposition 2 states that singular controls in
q. In order to get the value of Π(1), it suffices, at least formally, to the case of convex surfaces rolling on the plane are exactly
solve the following initial-value problem defined in the control straight lines on the plane.
space H

dΠ dπ IV. NUMERICAL IMPLEMENTATION


(s) = P (Π(s)) · (s) (13)
ds ds In this section, we describe how the CM can be implemented
initial
Π(0) = u . in order to solve numerically the MPP for rolling bodies in the
Therefore, to successfully apply the CM to the MPP, we have case where S1 is a strictly convex surface of IR3 and S2 is the
to resolve the following two issues. Euclidean plane IR2 . In that case, the dynamics of the control
1) Nondegeneracy: The path π has to be chosen so that, for system is given in geodesic coordinates by
every s ∈ [0, 1], Dφp Π(s)) always has full rank.
2) Nonexplosion: To solve (9), the PLE defined in (12) must v̇2 = u1 , ẇ2 = u2
have a global solution on [0, 1].
v̇1 = cos ψu1 − sin ψu2
Remark 4: Point 1) guarantees the existence of P (Π(s)) for
every s ∈ [0, 1] so that (12) is always well defined. Point 2) is 1 1
ẇ1 = − sin(ψ)u1 − cos(ψ)u2
also important since we need to evaluate Π(1) to get a control B B
steering the system from p to q. Bv 1 Bv 1
ψ̇ = − sin(ψ)u1 − cos(ψ)u2 (14)
Remark 5: We note that local existence and uniqueness of the B B
solution of the PLE hold as soon as φp is of class C 2 .
where we use B to denote the function occurring in the definition
It is reasonable to expect difficulties with the singular points
of geodesic coordinates on S1 . Note that (14) is deduced from
of φp , i.e., the controls v ∈ H, where rank Dφp (v) < 5. Let Sp
(6) by assuming that S2 is flat.
and φp (Sp ) be the set of singular points of φp and the set of
For the sake of simplicity, we make assumption that S1 is
singular values, respectively. The application of the CM to the
defined as one bounded connected component of the zero-level
MPP is thus decomposed in two steps. In the first one, we have
set of a smooth real-valued function f : IR3 → IR. The normal
to characterize (when possible) Sp and φp (Sp ). The second step
vector field to S1 is denoted by n : S1 → S 2 and is given in
consists of lifting paths π : [0, 1] → M avoiding φp (Sp ) to paths
that case by ∇f / ∇f , where ∇f = (fx , fy , fz ) denotes the
Π : [0, 1] → H globally defined on [0, 1] by (12). A sufficient
gradient vector of f . The Gaussian curvature of S1 is denoted
condition resolving 1) and 2) is as follows.
by K and we assume that Km in := minS 1 K > 0. In addition,
Condition 1: We say that a closed subset K of M verifies this
set Km ax := maxS 1 K.
condition if
In the sequel, we still use H and M to denote, respectively, the
1) K is disjoint from φp (Sp ), where φp (Sp ) is the closure of
control space and the state space of the control system defined
φp (Sp );
by (14). From Section III, we deduce the following motion-
2) there exists cK > 0 such that, for every u ∈ H with
planning algorithm, which, for any pair (p, q) ∈ M × M , pro-
φp (u) ∈ K, we have
duces an input ufinal steering the control system (14) from p to q.
P (u) ≤ cK u Note that the only difficulty in Algorithm 1 is step (iii), which
requires the numerical solution of an ordinary differential equa-
where P (u) = (inf z =1 z T Dφp (u)Dφp (u)T z)−1/2 , with
tion defined on the control space H, which is, in general, an
z ∈ Tφ∗p (u ) M .
832 IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 5, OCTOBER 2010

evaluation of a right inverse of Dφp (u) and the integration of


(14).

B. Computing Dφp (u)


We first need to define a field of covectors along γp,u . For
z ∈ Tφ∗p (u ) M , let λz ,u : [0, 1] → T ∗ M be the field of covectors
along γp,u satisfying (in coordinates) the adjoint equation along
γp,u with terminal condition z, i.e., λz ,u is a.c., λz ,u (1) = z,
and for almost every t ∈ [0, 1], we have
2

λ̇z ,u (t) = −λz ,u (t) · ui (t)DFi (λz ,u (t)) . (16)
i=1

If X is a smooth vector field over M , the switching function


infinite-dimensional vector space. In the following paragraphs, ϕX ,z ,u (t) associated with X is the evaluation of λ · X(x), and
we detail some key points to solve (15). the Hamiltonian function of X along (γp,u , λz ,u ), i.e., for t ∈
[0, 1], is given by
A. Discretizing the Control Space H ϕX ,z ,u (t) := λz ,u (t) · X(γp,u (t))
We start by approximating the control space H, which
(for more details, see, for instance, [8]). Then, Dφp (u) can be
is an infinite-dimensional vector space. Recall that, in our
computed as follows: For z ∈ Tφ∗p (u ) M and u, v ∈ H, we have
case, controls are just plane curves c2 : [0, 1] → IR2 such that
ċ2 = (u1 , u2 ) for almost all t ∈ [0, 1]. We divide the interval z · Dφp (u)(v) = (v, ϕz ,u )H (17)
[0, 1] into N parts and approximate the control space H by
the 2N -dimensional subspace Ĥ of piecewise-linear functions. where the switching-function vector ϕz ,u (t) is the solution of
Then, c2 can be approximated by ĉ2 , i.e., the linear interpo- the following Cauchy problem, which is defined (in coordinates)
2 ), where c2 = c2 (i/(N − 1)) = (xi , yi ) .
lation of (c12 , . . . , cN i T by (see [7])
On each segment [ti , ti+1 ] = [i/(N − 1), i + 1/(N − 1)], the ϕ̇1 = −u2 Kϕ3 , ϕ̇2 = u1 Kϕ3
approximate control (ûi1 , ûi2 )T is proportional to the vector
(xi+1 − xi , yi+1 − yi )T . ϕ̇3 = −u2 ϕ4 + u1 ϕ5 , ϕ̇4 = −u2 Kϕ3
Remark 6: We have chosen the space of piecewise-linear ϕ̇5 = u1 Kϕ3 . (18)
functions as the approximate control space for two reasons: 1)
Piecewise-linear curves are easy to be implemented on the plane; with terminal condition ϕz ,u (1) = z.
2) the corresponding trajectories on S1 are also easy to obtain by In practice, since the discrete Dφp (u) is a 5 × 5 matrix and its
integrating some geodesic equations by using Proposition 1 (see image is given by (17), it suffices to take five independent vectors
also Remark 2), instead of (14), where the function B, which in IR5 as final conditions z, for instance, the five elements in the
defines the geodesic coordinates, is not given explicitly. This canonical basis of IR5 , and integrate (18) in reverse time. In our
second point plays a crucial role in improving the efficiency of simulations, a fourth-order Runge–Kutta numerical scheme is
our method. More details will be given in Section IV-C. used for integration, the scalar product (·, ·)H in control space H
Remark 7: We note that when elements in Ĥ are piecewise- is evaluated by Gaussian quadrature, and the Gaussian curvature
linear functions with more than one piece, then they are not K is computed by using the following proposition.
singular inputs; see also Proposition 2. Proposition 3 (see[3]): Let S be (a bounded connected com-
The PLE (12) tells us how we have to modify this piecewise- ponent of) the zero-level set of f : IR3 → IR, and define a, b,
constant control (û1 , û2 ) in order to obtain an appropriate con- and c by
trol steering for our system from an initial state to a preassigned 2
∇ f − λI ∇f
final state. Under some general geometric assumptions for S1 , det = a + bλ + cλ2 (19)
theoretical results presented in Section III guarantee that, what- (∇f )T 0
ever the starting control we choose, (12) is complete and pro-
vides a correct control law at the end of the integration. We where ∇2 f is the matrix of the second derivatives of f , and I3
use the classical Euler scheme to integrate (12). Note that [8, the identity 3 × 3 matrix. With this notation, we have
Th. 1] ensures that, once there exists a global solution to (12), a/c
then for any “reasonable” Galerkin approximation of the control K= .
∇f 2
space and “reasonable” numerical scheme for the derivatives,
there exists a global solution for the corresponding numerical Explicit computations show that c = − ∇f 2 , and
approximation of (12). 2
∇ f ∇f
In the following two paragraphs, we give details about the a = det .
two key points for the numerical implementation, which are the (∇f )T 0
ALOUGES et al.: MOTION-PLANNING ALGORITHM FOR THE ROLLING-BODY PROBLEM 833

Hence, we have

∇2 f ∇f
det
(∇f )T 0
K=− . (20)
∇f 4
The gradient vector ∇f is then evaluated by a classical right-
shifting finite-difference scheme and ∇2 f by a centered one.
For example, if X = (x, y, z), then fx (X) is given by
f (x + ε, y, z) − f (x, y, z)
(21)
ε
and fxx (X) by
f (x + ε, y, z) − 2f (x, y, z) + f (x − ε, y, z) Fig. 2. Initial and final positions of contact point and orientations of the
(22) flattened ball (s = 0).
ε2
with ε > 0 small enough.
Finally, summing up (24) and (26) together, we get
C. Lifting the Plane Curve ĉ2 on S1 ċT ∇2 f (c)ċ
c̈ = − ∇f (c). (27)
Note that the curvature K appearing in (18) is taken at the ∇f (c) 2
final contact point on the surface S1 after it has rolled along
We use again a fourth-order Runge–Kutta scheme for numer-
the piecewise-constant curve ĉ2 . Thus, in order to locate the
ical integration of (27). An additional difficulty is that the nu-
final point, we need to “lift” the plane curve ĉ2 on S1 , and the
merical integration is not performed in an Euclidean space, but
lifting dynamics are given by (14). However, since the geodesic
on a manifold S1 , t > 0. Assuming that we are at point x ∈ S1
coordinates involved in (14) are not given explicitly in practice,
at time t, then, at time t + δt, we move to Xnew = X + (δt)d,
our numerical lifting method is based on Proposition 1 (see also
with d ∈ Tx S1 , but Xnew does not belong to S1 if d is nonzero.
Remark 2).
Therefore, at each integration step, we have to “project” Xnew
On each interval [ti , ti+1 ], the approximate control curve ĉ2 is
on S1 . More precisely, let us assume that the point (0, 0, 0) is
a straight line (i.e., a geodesic in IR2 ), and then, by Proposition
inside the convex body S1 . Since S1 is defined as (a bounded
1, the lifting curve ĉ1 on S1 is also a geodesic on each inerval
connected component of) the zero-level set of a smooth function
[ti , ti+1 ] for all i = 0, . . . , N − 1. Then, from the initial contact
f , we assume that |f (Xnew )| ≤ ε for some ε  1, i.e., Xnew is
point X0 on S1 , we can integrate successively the geodesic
close to S1 . Then, there exists a unique real number µ close to 1
equation on each [ti , ti+1 ] with initial conditions equal to ĉ1 (ti )
such that f (µXnew ) = 0, as a simple consequence of the con-
and (ûi1 , ûi2 ), for i = 0, . . . , N − 1.
vexity of S1 . The “projection” issue to be addressed is clearly a
Let us write explicitly the geodesic equation to be integrated;
local one, and therefore, Newton’s method is efficient for find-
for more details, see, for instance, [3]. Recall that a curve c :
ing µ. The derivative with respect to µ is also needed, which is
[0, 1] → S1 is a geodesic curve if it verifies (1). In the case
evaluated by a finite-difference scheme similar to (21).
where S1 is an immersed surface in IR3 , (1) is equivalent to

c̈(t) ⊥ Tc(t) S1 (23) V. SIMULATIONS

for almost all t in [0, 1]. We have applied the numerical CM presented above for MPP
When S1 is defined as (a bounded connected component of) of several bodies rolling on the Euclidean plane. We first present
the zero-level set of a real-valued function f : IR3 → IR, we the rolling of a flattened ball and an egg. We then simulate a case
have ∇f (x) ⊥ Tx S1 at every x ∈ S1 , t > 0. Thus, (24) becomes where the rolling body does not have a symmetry of revolution.
 Still, the CM works quite efficiently.
∇f (c) ∇f (c) Let us point out that we have written a MATLAB program
c̈ = c̈, (24)
∇f (c) ∇f (c) that provided us with the figures presented below (see Figs. 2–
10). In particular, these figures contain buttons and windows
where ,  is the scalar product in IR3 . of the MATLAB interface. For the convenience of the reader,
Furthermore, since c is a curve traced on S1 , we also have we will recall all the equations defining the rolling surfaces in
the corresponding paragraphs. All the figures show the starting
ċ(t), ∇f (c(t)) = 0 (25)
and ending contact points and orientations on the top left, the
for almost all t in [0, 1]. Then, by deriving (25) with respect to current trajectory on the plane together with the starting and
t, we get ending configurations of the rolling body on the top right, and
the corresponding trajectory on the body on the bottom left.
c̈, ∇f (c) + ċ, ∇2 f (c)ċ = 0. (26) Since the key point is to show how the CM smoothly modifies
834 IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 5, OCTOBER 2010

Fig. 3. Computation by CM (s = 70).

Fig. 5. Initial and final positions of contact point and orientations of the egg
(s = 0).

Fig. 4. Matching at the end of rolling.

an arbitrary nonsingular plane curve to achieve a “right” one,


for each test case, we show in the first figure the initial curve
that we have chosen, in the second figure the curve computed Fig. 6. Computation by CM (s = 70).
by the CM, and in the last figure the matching between the real
final state and the preassigned one. The computation time is We note that ∇f (x, y, z) = (2x/(1 − 0.4z), 2y/(1 − 0.4z),
on average 30 s (2.2-GHz Intel Core 2 Duo, 1.6-G memory) 0.4(x2 + y 2 )/(1 − 0.4z)2 + (z/2))T . One can check that it is
for 70 iterations with N = 100 for the discretization of control never equal to zero on the zero-level set of (29), and therefore,
space H. (20) and (27) are always well defined.

A. Flattened Ball Rolling on the Plane


This flattened ball is defined by the zero-level set of the C. More General Case
function In Section III, the global convergence of CM has been proven
for rolling of convex body with symmetric axis. However, we
f (x, y, z) = x2 + y 2 + 5z 2 − 1. (28)
show in the subsequent simulations that the CM still works
The gradient ∇f (x, y, z) is equal to (2x, 2y, 10z)T . One can numerically in more general cases, even though a theoretical
check that it is never equal to zero on the zero-level set of (28). convergence result is not available. This illustrates the robust-
Then, (20) and (27) are always well defined. ness of the method.
For example, we take the convex body without symmetric
axis, which is defined by one bounded connected component of
B. Egg Rolling on the Plane
the zero-level set of the function
This “egg” is defined by one bounded connected component
of the zero-level set of the function x2 2y 2 0.5z 2
f (x, y, z) = + + − 1.
x2 + y 2 z2 1 − 0.5y 1 − 0.1z 1 − 0.3x − 0.1y
f (x, y, z) = + − 1. (29) (30)
1 − 0.4z 4
ALOUGES et al.: MOTION-PLANNING ALGORITHM FOR THE ROLLING-BODY PROBLEM 835

Fig. 9. Computation by CM (s = 70).

Fig. 7. Matching at the end of rolling.

Fig. 10. Matching at the end of rolling.

this point of view, exact computations, as presented in [14],


cannot be extended beyond the plate–ball system, and approx-
Fig. 8. Initial and final positions of contact point and orientations of the convex imate computations based on some discretization of the state
body (s = 0).
space presented in [4] produce highly oscillating trajectories
We note that as they are obtained by concatenating a large number of basic
 2x 0.3  actions composed of rolling along some closed curves defined
1−0.5y + (1−0.3x−0.1y ) 2
 4y  on S1 .
 0.1 
∇f (x, y, z) =  1−0.1z + (1−0.3x−0.1y ) 2 . In this paper, we have adopted a more global point of view,
 2
 which is to modify continuously, via the CM developed in [7]
0.2y z
(1−0.1z ) 2 + 1−0.3x−0.1y and [8], an arbitrary nonsingular control (any plane curve, which
is not straight line in the case of problem L1) in order to achieve
One can check that it is never equal to zero on the zero-level set
one control that steers the system from a given initial state to
of (30), and therefore, (20) and (27) are always well defined.
preassigned final state. We have implemented this method to
solve the problem L1-1 (i.e., rolling of general strictly convex
VI. DISCUSSION AND CONCLUSION
bodies on the free plane). We have shown through several exam-
The main difficulty in the motion planning for convex bodies ples the robustness and the convergence speed of this method. It
with rolling constraints relies on the fact that the displacement is worth pointing out that the only knowledge about the surface
and the change of orientation cannot be dissociated one from S1 required by the numerical implementation of our method is
the other. In the case of problem L1, every closed curve on the Gaussian curvature function K1 of S1 . We have assumed
S1 can be associated with an element of the group SE(2), in Section IV that there exists a smooth function f : IR3 → IR
and the concatenation of two closed curves corresponds to the such that S1 = f −1 (0); then, K1 can be directly expressed (and
group operation for SE(2). This correspondence was implicitly numerically evaluated) from f . The numerical advantage of this
mentioned in [14] via the Gauss–Bonnet theorem, and it was level-set approach relies on the fact that our motion-planning
explicitly and systematically explored in [4] for the construc- algorithm can be implemented without dealing with any change
tion of the lattice structure and basic actions. However, using of local parameterization (chart) of S1 . We also note that this
836 IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 5, OCTOBER 2010

assumption is not restrictive. Indeed, for any compact convex [16] R. M. Murray, Z. Li, and S. S. Sastry, A Mathematical Introduction to
body S1 , if we assume that the origin 0 is inside of S1 , then we Robotic Manipulation. Boca Raton, FL: CRC, 1994.
[17] L. A. Pars, A Treatise on Analytical Dynamics. London, U.K.: Heine-
can define f as follows: mann, 1965.
[18] D. J. Montana, “The kinematics of contact and grasp,” Int. J. Robot. Res.,
x vol. 7, pp. 17–32, 1988.
f (x) = t − 1, if ∈ S1 . (31) [19] M. Spivak, A Comprehensive Introduction to Differential Geometry III.
t Berkeley, CA: Publish or Perish, 1975.
[20] H. J. Susmmann, “New differential geometric methods in nonholonomic
path finding,” in Systems, Models, and Feedback: Theory and Applica-
Then, S1 = f −1 (0). Moreover, one can show that f is convex. tions. A. Isidori and T. J. Tarn, Eds. Boston, MA: Birkhäuser, 1992,
Therefore, the Gaussian curvature can be computed from this pp. 365–384.
function f since any continuous convex function admits second [21] H. J. Sussmann, “A continuation method for nonholonomic path-finding
problems,” presented at the IEEE Conf. Dec. Control, San Antonio, TX,
derivatives almost everywhere (see [12, Ch. I, Sec. V]). 1992.
Our method can be adapted in order to solve L1-2 (i.e., a [22] K. Tchon and J. Jakubiak, “Endogenous configuration space approach to
convex body S1 rolling on a plane with obstacles on the lat- mobile manipulators: A derivation and performance assessment of Jaco-
bian inverse kinematics algorithms,” Int. J. Control, vol. 76, pp. 1387–
ter) by potential fields, which is the purpose of a forthcoming 1419, 2003.
paper. Solving L1-3 (i.e., strictly convex body S1 rolling on a [23] K. Tchon and L. Malek, “Singularity robust Jacobian inverse kinematics
plane with prohibited regions on S1 ) is more challenging since for mobile manipulators,” in Advances in Robot Kinematics. New York:
Springer-Verlag, 2008, pp. 155–164.
we must deal numerically with local charts of S1 . A possible [24] T. Wazewski, “Sur l’évaluation du domaine d’existence des fonctions
way to address this issue is to use penalization techniques. An implicites réelles ou complexes,” Ann. Soc. Polon. Math., vol. 20, pp. 81–
additional difficulty regarding L2 (i.e., one strictly convex body 125, 1947.
rolling on the top of the other without or with prohibited regions)
relies on the fact that the invertibility of Dφp involved in the PLE
(13) as well as the nonexplosion condition require K2 − K1 = 0 François Alouges was born in 1966 in France.
at the contact point, but this condition may not be globally ver- He received the Ph.D. degree in mathematics from
Orsay University, Paris, France, in 1990.
ified for two general smooth convex bodies. He is currently a Professor of applied mathemat-
ics with the Ecole Polytechnique, Palaiseau, France.
REFERENCES He has been involved in control theory, mainly from
a practical viewpoint. His research interests include
[1] A. Agrachev and Y. Sachkov, “Control theory for the geometric view- numerical analysis, scientific computing, and partial
point,” in Control Theory and Optimization II. Encyclopedia of Math- differential equations.
ematical Sciences. New York: Springer-Verlag, 2004.
[2] E. L. Allgower and K. Georg, “Continuation and path following,” Acta
Numer., vol. 2, pp. 1–64, 1992.
[3] M. Berger and B. Gostiaux, “Differential geometry: Manifolds curves, sur-
faces,” in Graduate Texts in Mathematics. New York: Springer-Verlag,
1988.
[4] A. Bicchi and A. Marigo, “Dexterous grippers: Putting nonholonomy to
work for fine manipulation,” Int. J. Robot. Res., vol. 21, pp. 427–442, Yacine Chitour was born in Algiers, Algeria, in
May/Jun. 2002. 1968. He received the Ph.D. degree in mathemat-
[5] C. Cai and B. Roth, “On the spatial motion of rigid bodies with point ics from Rutgers University, New Brunswick, NJ, in
contact,” in Proc. IEEE Conf. Robot. Autom., 1987, pp. 686–695. 1996.
[6] L. Cui and J. S. Dai, “A Darboux-frame-based formulation of spin-rolling He is currently a Professor of control theory
motion of rigid objects with point contact,” IEEE Trans. Robot., vol. 26, with the Université Paris-Sud XI, Gif-sur-Yvette,
no. 2, pp. 383–388, Apr. 2010. France, where he is a member of the Labora-
[7] A. Chelouah and Y. Chitour, “On the motion planning of rolling surfaces,” tiore des Signaux et Systèmes, Centre National
Forum Math., vol. 15, no. 5, pp. 727–758, 2003. de la Recherche Scientifique and École Supérieure
[8] Y. Chitour, “A continuation method for motion-planning problems,” d’Électricité. His research interests include finite- and
ESAIM Control Optim. Calc. Var., vol. 12, no. 1, pp. 139–168, 2006, infinite-dimensional geometric control.
Electronic.
[9] Y. Chitour, “Path planning on compact Lie groups using a homotopy
method,” Syst. Control Lett., vol. 47, no. 5, pp. 383–391, 2002.
[10] Y. Chitour and H. J. Sussmann, “Line-integral estimates and motion plan-
ning using the continuation method,” in Essays on Mathematical Robotics Ruixing Long was born in Nanjing, China, in 1984.
(Minneapolis, MN, 1993), (IMA Math. Appl. 104). New York: Springer- He received the Dipl.Ing. degree from ENSTA Paris-
Verlag, 1998, pp. 91–125. Tech, Paris, France, in 2007 and the M.S. degree
[11] A. Divelbiss and J. T. Wen, “A path space approach to nonholonomic in mathematics from the Université Paris-Sud XI,
motion planning in the presence of obstacles,” IEEE Trans. Robot. Autom., Gif-sur-Yvette, France. He is currently working to-
vol. 13, no. 3, pp. 443–451, Jun. 1997. ward the Ph.D. degree with the Ecole Polytechnique,
[12] J.-B. Hiriart-Urruty and C. Lemaréchal, Convex Analysis and Minimiza- Palaiseau, France.
tion Algorithm. (A Series of Comprehensive Studies in Mathematics His research interests include the motion-planning
305). New York: Springer-Verlag, 1993. problem for general nonholonomic systems.
[13] W. Klingenberg, “Riemannian geometry,” in Studies in Mathematics.
Berlin, Germany: De Gruyter, 1982.
[14] Z. Li and J. Canny, “Motion of two rigid bodies with rolling constraint,”
IEEE Trans. Autom. Control, vol. 6, no. 1, pp. 62–72, Feb. 1990.
[15] A. Marigo and A. Bicchi, “Rolling bodies with regular surface: Control-
lability theory and applications,” IEEE Trans. Autom. Control, vol. 45,
no. 9, pp. 1586–1599, Sep. 2000.