Planning Safe Cyclic Motions under Repetitive Task Constraints
Massimo Cefalo, Giuseppe Oriolo, Marilena Vendittelli
Abstract— We consider motion planning in the presence
of obstacles for redundant robotic systems subject to repet-
itive task constraints. For this open problem, we present
a novel control-based randomized planner which produces
cyclic, collision-free paths in configuration space and guarantees
continuous satisfaction of the task constraints. In particular,
the proposed algorithm relies on bidirectional search and loop
closure in the task-constrained configuration space. Planning
experiments on a simple 3R planar robot and the KUKA LWR-
IV 7-dof manipulator are reported to show the effectiveness of
the proposed method.
I. INTRODUCTION
The operation of robotic systems invariably involves some
form of task constraint. In the industrial context, typical
tasks concern the manipulator end-effector, which should
be placed at a certain position and/or orientation (e.g., for
picking an object), or follow a certain path/trajectory (e.g.,
for cutting or welding). In service robotics, more general
tasks can be assigned, ranging from tracking visual features
to maintaining mechanical balance. A special case, which
is particularly relevant in industrial applications but may
also arise in other situations, is that of repetitive tasks; i.e.,
tasks that must be repeated over and over, and are therefore
described by closed paths in the task space.
Redundant robots possess more degrees of freedom than
those strictly necessary to accomplish a given task, and
therefore offer increased dexterity and flexibility for pur-
suing additional objectives, among which one of the most
important is avoidance of workspace obstacles. Traditionally,
task-constrained motion in configuration space for redundant
robots is generated through kinematic control techniques [1],
[2], [3], [4]. These are on-line motion generation schemes
that use a generalized inverse of the task Jacobian (e.g.,
the pseudoinverse), possibly with the addition of internal
motions that do not perturb the execution of the task (null-
space motions) and are therefore used for local optimization.
However, researchers readily identified a critical issue of
these schemes when used on repetitive tasks: in general,
closed paths in the task space do not result in closed paths in
the joint space. This is clearly a drawback, because it means
that the robot motion is essentially unpredictable from cycle
to cycle. Such behavior is particularly undesirable in human-
robot interaction scenarios, because it ultimately hinders the
legibility of the robot movements by the human.
The authors are with the Dipartimento di Ingegneria Informatica, Auto-
matica e Gestionale, Sapienza Universit
`
a di Roma, Via Ariosto 25, 00185
Rome, Italy. E-mail: {cefalo,oriolo,venditt}@dis.uniroma1.it. This work is
supported by the EU FP7 ICT-287513 SAPHARI project.
With reference to the case of pure generalized inversion
(no null-space motions), Shamir and Yomdin [5] identified
a quite restrictive structural condition that the generalized
inverse must satisfy in order to possess the cyclicity (also
called repeatability) property. This condition, which was fur-
ther refined in [6], was exploited to design cyclic generalized
inverses, e.g., in [7] and to achieve asymptotically cyclic
kinematic control in [8]. Other works on cyclicity include,
e.g., [9], [10], [11]. However, even when a cyclic generalized
inverse is used, there is no space left for additional objectives
such as obstacle avoidance, because the addition of null-
space motions would destroy the cyclicity property.
An exception to the above situation is represented by
kinematic control schemes that rely on task augmentation
to enforce a one-to-one mapping between task and configu-
ration space. The archetype of this approach is the Extended
Jacobian method [12], [13]. This technique is guaranteed
to produce cyclic configuration paths in response to closed
task paths, but only on the condition that algorithmic singu-
larities are not encountered. Even neglecting for a moment
this pitfall, however, it is impossible to guarantee that the
solution paths are collision-free, essentially due to the fact
that obstacle avoidance cannot be effectively encoded in an
additional equality task.
An important observation is that repetitive tasks are usu-
ally assigned and known in advance. We argue, therefore,
that off-line planning is the best approach to generate safe
cyclic paths in configuration space when faced with this kind
of tasks. In the literature, under the name (Task-)Constrained
Motion Planning (for short, (T)CMP) one may find several
methods for generating collision-free robot motions in the
presence of (task) constraints; examples include [14], [15],
[16], [17], [18], [19]. These techniques, however, do not
consider the additional requirement that the paths in con-
figuration space must be cyclic.
The solution approach proposed in this paper builds upon
our previous work [17], in which a randomized planner
was presented for solving the TCMP problem. In particular,
a specialized motion generation scheme was introduced
to guarantee continued satisfaction of the task constraint
throughout the motion. Here we use the same basic scheme,
but to guarantee that the generated paths in configuration
space are cyclic, a bidirectional search is performed by
growing two Rapidly-exploring Random Trees in the task-
constrained configuration space: the first proceeds in the
forward direction of the task space path, whereas the second
moves backwards. When the two trees become sufficiently
near, they are joined by a loop closure procedure that is
2013 IEEE International Conference on Robotics and Automation (ICRA)
Karlsruhe, Germany, May 6-10, 2013
978-1-4673-5643-5/13/$31.00 ©2013 IEEE 3807
designed using a feedback control technique.
The paper is organized as follows. Section II introduces
the basic framework and formally defines the problem. The
proposed solution method is described in Section III while
the results of some planning experiments are presented in
Section IV to illustrate its performance. Some future work
is mentioned in the concluding section.
II. PROBLEM FORMULATION
Since the problem addressed in this paper is a special
case of Task-Constrained Motion Planning (TCMP), we will
adopt the terminology and framework introduced in [17].
With respect to that work, which considers general robotic
systems, here it is assumed for simplicity that the robot is
not subject to nonholonomic constraints. This means, for
example, that wheeled mobile manipulators are excluded
from the developments to be presented. The extension to
such systems is however relatively straightforward and is the
subject of ongoing work.
Consider a robot whose configuration q takes value in
an n
q
-dimensional configuration space C. The robot body
moves in a workspace W (a subset of IR
2
or IR
3
) that
contains obstacles. Since no differential constraints act on the
system, its kinematic model consists of simple integrators.
For path or motion planning purposes, it is appropriate to
use the geometric form of such model, which specifies the
admissible tangent vectors to a configuration space path q(s),
where s is a path parameter, as
q
0
=
˜
v, (1)
with the notation ( )
0
= d( )/ds. This model simply ex-
presses the fact that the generalized coordinates q can move
arbitrarily in C. The tilde over the v’s indicates that these
are geometric inputs, rather than velocity inputs. If s = t, a
trajectory is directly planned rather than a path.
Consider now a robot task expressed in coordinates t,
which take values in an n
t
-dimensional task space T . The
t coordinates depend on the q coordinates through the
kinematic map
t = f(q). (2)
At the differential level, it is
t
0
= J(q)q
0
= J(q)
˜
v,
where J(q) = df/dq is the n
t
× n task Jacobian which
maps geometric inputs (configuration-space tangent vectors)
to task-space tangent vectors.
We will assume that n
q
> n
t
, i.e., the robot is kinemat-
ically redundant with respect to the task of interest. This
means that the inverse image
¯
q = f
−1
(
¯
t) in C of a point
¯
t in T is not unique in general. In particular, this image
may be (a) an (n
q
− n
t
)-dimensional subset of C, consisting
of one or more disjoint manifolds, or (b) a finite number
of configurations [20]. Task points that map to the first
group include regular and coregular points (the latter are
also called avoidable singularities), whereas only singular
points (unavoidable singularities) map to the second group.
Assume now that the assigned task is a closed path t
d
(s),
with s ∈ [0, 1] w.l.o.g. and t
d
(0) = t
d
(1). For technical
reasons, we will suppose that t
d
(s) is differentiable and, for
the problem to be well-posed, that:
t
d
(s) ∈ T
∗
, ∀s ∈ [0, 1],
where T
∗
is the non-singular task space, defined as the set
of regular and coregular task points of T . The initial joint
configuration q
start
is assigned
1
, with f (q
start
) = t
d
(0).
The Cyclic Task-Constrained Motion Planning (C-TCMP)
problem consists in finding a configuration space path
q(s), s ∈ [0, 1], such that:
1) t(s) = f (q(s)) = t
d
(s), for s ∈ [0, 1] (the desired
path is executed in T );
2) q(0) = q(1) (the motion is cyclic in C);
3) the robot does not collide with obstacles or with itself.
The planning space for the C-TCMP problem is
C
task
= {q ∈ C : f (q) = t
d
(s), for some s ∈ [0, 1]}.
The manifold C
task
, called task-constrained configuration
space in the following, is a natural foliation, with s as a
parameter. A generic leaf is defined as
L(s) = {q ∈ C : f (q) = t
d
(s)},
and we have
C
task
=
[
s∈[0,1]
L(s).
Since the desired task path is closed, we have L(0) = L(1).
The existence of a solution to the C-TCMP problem
depends on the obstacle placement, and in particular by
the connectivity of C
free
∩ C
task
, the portion of the free
configuration space that is compatible with the task path
constraint.
III. THE C-TCMP PLANNER
Our approach to the solution of the C-TCMP problem is
illustrated in Fig. 1. In summary, a bidirectional search of
the task-constrained configuration space C
task
is performed
by growing two Rapidly-exploring Random Trees (RRTs,
see [21]). Both trees are rooted at q
start
, i.e., on L(0); but the
first explores C
task
in the direction of increasing s, whereas
the second proceeds in the opposite direction. In view of the
constraint represented by the desired task path, tree extension
in our planner is obtained via a control-based variation [17]
of the basic RRT mechanism. When the two trees become
sufficiently near, they are joined by a loop closure procedure.
For building the trees, we sample the desired task path
t
d
(s) using a sequence {s
0
= 0, s
1
, . . . , s
N−1
, s
N
= 1} of
N + 1 values of the path parameter s. Correspondingly, we
use the shorthand notations t
d,i
= t
d
(s
i
) and L
i
= L(s
i
).
Recall that it is t
d,0
= t
d,N
and L
0
= L
N
.
1
If needed, q
start
can be preliminarily computed by inverse kinematics.
3808
q
L
closed task path
s
0
1
2
N-1
N-2
L
L
L
L
L
L
k
k+1
loop closure
backward tree
forward tree
start
q
bw
q
fw
s
s
s
s
s
s
s
1
2
N-1
N-2
k+1
k
L
0
N
=
N
s
,
Fig. 1. The proposed C-TCMP planner relies on bidirectional search and loop closure in the task-constrained configuration space.
A. Motion Generation
The tree edges are subpaths in configuration space produced
by using the following motion generation scheme:
q
0
=
˜
v (3)
˜
v = J
†
(q)(t
0
d
+ k
t
e
t
) + (I − J
†
(q)J(q))
˜
w, (4)
where J
†
is the pseudoinverse of the task Jacobian J, k
t
is a positive gain, e
t
= t
d
− t is the task error, I − J
†
J
is the orthogonal projection matrix in the null space of J,
and
˜
w is an arbitrary n
q
-vector that represents a residual
input. One has J
†
= J
T
(JJ
T
)
−1
when J is full row rank;
in our planner, this assumption is always satisfied because
singular configurations are discarded (see Sect. III-B). Use of
the above scheme guarantees e
0
t
= −k e
t
, i.e., exponentially
stable
2
tracking of the desired task path, regardless of the
choice of
˜
w.
For any choice of
˜
w(s), s ∈ [0, 1], and starting from a
generic leaf of C
task
, integration of (3–4) provides a config-
uration path that starts from the current leaf and traverses
subsequent leaves, always remaining in C
task
. A numeric
solver can be used to actually perform the integration.
In principle, one could use a different parameterization
for the task space and configuration paths [17]. This would
allow, for example, to perform a self-motion at the configu-
ration level so as to modify the robot internal posture while
the task variables do not change. In this paper, we shall not
exploit this opportunity, and will avoid self-motions in order
to produce smoother paths.
2
Since this is a planning scheme, stability is required for reducing the
drift associated to a numerical integration of (3–4).
B. Tree Extension
The forward tree T
fw
is rooted at q
start
. At each iteration,
a random configuration q
rand
is first generated
3
in C
task
;
this is done by picking a value for s ∈ {s
1
, . . . , s
N−1
},
and then choosing one of the inverse kinematic solutions
for the corresponding sample of t
d
(s). A scan of T
fw
is
then performed to identify its vertex (configuration) that is
closest to q
rand
; call q
near
this configuration and s
i
the
parameter value associated to the leaf L
i
where q
near
is
located. At this point, a fixed number of different constant
values are randomly generated for the residual vector
˜
w,
with the constraint that the norms of the two terms in the
rhs of (4) are in a certain proportion. For each value of
˜
w, the motion generation scheme (3–4) is then integrated
numerically starting from q
near
at s = s
i
and ending at
s = s
i+1
. Only the value of
˜
w that results in the final
configuration closest to q
rand
is retained. The corresponding
subpath joins q
near
, belonging to L
i
, to a new configuration
q
new
, that belongs to the next leaf L
i+1
.
Once the subpath has been generated, it is checked for
collision with obstacles as well as for the occurrence of
singularities. If the test is passed, q
new
and the subpath from
q
near
to q
new
are added to T
fw
as a vertex and an edge,
respectively.
The extension mechanism of the backward tree T
bw
is
identical, with the only difference that the integration is
performed backwards, i.e., over decreasing values of s. That
is, edges of from T
bw
starting from q
start
on L
0
= L
N
will
lead to vertices on L
N−1
, and so on (see Fig. 1). Recall
3
Our experiments have shown that generating q
rand
in C
task
, although
more expensive in itself, improves considerably the overall performance of
the planner with respect to simply taking q
rand
in C.
3809
that system (3–4) is symmetric, and therefore any backward
motion can be reversed to the direction where s increases.
We emphasize that, unlike sampling-based constrained
motion planners, the use of the motion generation scheme (3–
4) guarantees that the task variables move along the desired
path throughout the motion. In particular, the tracking ac-
curacy can be arbitrarily increased by reducing the stepsize
for integrating (3–4), without any consequence on the size
of the tree, to which only q
new
will be added in any case.
The planning algorithm proceeds by alternately growing
T
fw
and T
bw
. As customary in bidirectional search, extension
steps are occasionally replaced (a parameter controls this
event) by connection steps aimed at reducing the gap be-
tween the two trees. Whenever a pair of vertices is generated
that belong to different trees and lie on two adjacent leaves,
the algorithm invokes the loop closure procedure.
C. Loop Closure
As shown in Fig. 1, once two vertices (configurations) q
fw
∈
L
k
and q
bw
∈ L
k+1
have been generated by T
fw
and T
bw
,
respectively, they must be joined with a collision-free path
contained in C
task
in order to ‘close the loop’ and produce
a cyclic configuration space path that solves the C-TCMP
problem. In this phase, in place of (3–4) we use a different
motion generation scheme that is inspired to the self-motion
stabilization scheme proposed in [22].
The basic idea here is to identify a suitable subset of
n
q
− n
t
‘redundant’ configuration variables, and to perform
the desired reconfiguration on these variables. The remaining
‘base’ variables will move so as to ensure that the desired
task space subpath from t
d,k
to t
d,k+1
is followed. Under
certain conditions, this guarantees that the base variables will
also converge to their desired value. This can be formalized
as follows.
Partition the configuration vector q as (q
red
, q
base
), where
q
red
is (n
q
− n
t
)-dimensional and q
base
is n
t
-dimensional.
Correspondingly, the task Jacobian is also partitioned in an
n
t
× (n
q
− n
t
) submatrix J
red
= ∂f /∂q
red
and an n
t
× n
t
submatrix J
base
= ∂f /∂q
base
. Assume that the partition
has been made in such a way that J
base
is nonsingular at
q
fw
. Reconfiguration from q
fw
to q
bw
in C
task
can then be
obtained by letting
(q
red
)
0
= k
red
(q
red
bw
− q
red
) (5)
(q
base
)
0
= J
−1
base
(q)(t
0
d
+ k
t
e
t
− J
red
(q)(q
red
)
0
) (6)
where k
red
is a positive constant, q
red
bw
is the value of q
red
at
q
bw
, and t
0
d
indicates the geometric derivative of the desired
task path from t
d,k
to t
d,k+1
.
By integrating eqs. (5–6) from q
fw
at s = s
k
, the q
red
variables will converge exponentially to their destination
q
red
bw
, while the final value of the q
base
variables will belong
to the finite set of the inverse kinematic solutions corre-
sponding to t
d,k+1
. In particular, it is easy to prove that
q
base
will converge exactly to q
base
bw
provided that (i) q
base
fw
and q
base
bw
belong to the same class of kinematic solutions
(e.g., elbow-up or elbow down), and (ii) J
base
remains
nonsingular throughout the motion. Under these conditions,
therefore, the configuration variables will move from q
fw
to q
bw
while satisfying the task constraint, and hence the
loop will be effectively closed. If J
base
becomes singular
(or better, approaches a singularity) during the use of (5–6),
it is sufficient to change the partition of q and re-apply loop
closure from the current configuration.
In practice, the above conceptual scheme is used in our
planner without changing the partition of q. If J
base
becomes
singular, or if a collision is found, we simply abort the
loop closure phase and go back to the tree extension phase,
because the probabilistic completeness property of RRT
guarantees that sooner or later another pair (q
fw
, q
bw
) will
be generated on which loop closure will be successful.
When closure is obtained, a solution to the C-TCMP
problem is reconstructed by patching together the subpath
from q
start
to q
fw
on T
fw
, the loop closure subpath from
q
fw
to q
bw
, and the subpath from q
bw
to q
start
on T
bw
(in
the reverse direction).
IV. PLANNING EXPERIMENTS
The proposed C-TCMP planner has been implemented in
Kite, a cross-platform motion planning software produced
by Kineo CAM, on a 64-bit Intel Core i5-2320 CPU
running at 3 GHz. We report planning results for sce-
narios of increasing difficulty. The first involves a sim-
ple 3R planar manipulator, while the second and the
third refer to the KUKA LWR-IV 7-DOF robot. Anima-
tions of the generated motions are contained in the video
clip accompanying this submission and are available at
http://www.dis.uniroma1.it/˜labrob/research/C-TCMP.html.
In the first scenario, a 3R planar manipulator must move
its tip along an elliptical path while avoiding three small ob-
stacles. In this case, we have a single degree of redundancy.
We used
4
N + 1 = 11 equispaced samples of the desired
task path and generated motions using Euler’s method with
integration step ∆s = 0.002. The gain k
t
was set to 100.
The norm of the null space vector in eq. (4) is constrained
to be at most 150% of that of the first term. Figure 2 shows
a cyclic solution which was computed by our planner in
about 4 s (the average running time computed over ten runs
was around 5 s). The forward and backward trees contain at
the end 24 and 11 vertices, respectively. The mean and the
maximum value of the task error norm over the whole path
are, respectively, 0.0729 mm and 0.1354 mm, confirming that
the proposed method guarantees continued satisfaction of the
task constraint. Cyclicity in configuration space is confirmed
by Fig. 5, which shows that the displacement with respect
to the initial configuration returns to zero at the end of the
motion. For comparison, the accompanying video shows the
path produced by simple pseudoinverse control, which is not
cyclic — as expected — and also leads to collisions with
the obstacles.
In the second scenario, the KUKA LWR-IV must draw
a circle on a whiteboard while avoiding collisions with the
table on which it is mounted, the wall, the whiteboard itself
4
See [17] for a discussion on the choice of N .
3810
Fig. 2. Planning experiment on a 3R planar robot: Samples from the solution. The first and the last sample confirm that cyclicity has been achieved.
Fig. 3. First planning experiment on the LWR-IV: Samples from the solution. The first and the last sample confirm that cyclicity has been achieved.
Fig. 4. Second planning experiment on the LWR-IV: Samples from the solution. The first and the last sample confirm that cyclicity has been achieved.
(simple contact with the pen is obviously allowed) and an
articulated lamp protruding from the wall. Self-collisions are
also avoided. Since the drawing task is 3-dimensional, the
degree of redundancy is 6 − 3 = 3 (the wrist roll is frozen
because it is irrelevant for the task). The planner parameters
are the same of the previous case. The solution shown in
Fig. 3 took 83 s to compute, with a final size of the forward
and backward trees of 116 and 33 vertices, respectively. The
task error is again negligible (mean value 0.076 mm and
maximum value 0.1814 mm). Figure 6 shows that cyclicity
has been achieved also in this case.
The third scenario refers to a similar task (draw an ellipse
on the whiteboard) but the LWR-IV is now placed on the
opposite side of the wall and must maneuver through a
small window to reach the whiteboard. Using once again the
same parameters of the previous experiments, the solution of
Figs. 4 and 7 was obtained in 32 s (consider that the size
of C
task
∩ C
free
is significantly smaller than in the second
scenario). At the end, the forward and backward trees contain
29 and 34 vertices, respectively, while the mean and the
maximum task errors are 0.0275 mm and 0.0418 mm.
V. CONCLUSIONS
For redundant robotic systems subject to repetitive task
constraints, we have presented a control-based approach for
planning cyclic, collision-free configuration space paths. We
3811