ChapterPDF Available

Incremental Motion Reshaping of Autonomous Dynamical Systems

Authors:

Abstract and Figures

This paper presents an approach to incrementally learn a reshaping term that modifies the trajectories of an autonomous dynamical system without affecting its stability properties. The reshaping term is considered as an additive control input and it is incrementally learned from human demonstrations using Gaussian process regression. We propose a novel parametrization of this control input that preserves the time-independence and the stability of the reshaped system, as analytically proved in the performed Lyapunov stability analysis. The effectiveness of the proposed approach is demonstrated with simulations and experiments on a real robot.
Content may be subject to copyright.
Incremental Motion Reshaping of Autonomous
Dynamical Systems
Matteo Saveriano1* and Dongheui Lee2,3
1University of Innsbruck, Innsbruck, Austria,
matteo.saveriano@uibk.ac.at
2Technical University of Munich, Munich, Germany
3German Aerospace Center (DLR), Weßling, Germany
* This work was carried out when the author was at the Human-centered Assistive
Robotics, Technical University of Munich
Abstract. This paper presents an approach to incrementally learn a re-
shaping term that modifies the trajectories of an autonomous dynamical
system without affecting its stability properties. The reshaping term is
considered as an additive control input and it is incrementally learned
from human demonstrations using Gaussian process regression. We pro-
pose a novel parametrization of this control input that preserves the
time-independence and the stability of the reshaped system, as analyt-
ically proved in the performed Lyapunov stability analysis. The effec-
tiveness of the proposed approach is demonstrated with simulations and
experiments on a real robot.
Keywords: Incremental learning of stable motions ·Dynamical systems
for motion planning
1 Introduction
In unstructured environments, the successful execution of a task may depend on
the capability of the robot to rapidly adapt its behavior to the changing scenario.
Behavior adaptation can be driven by the robot’s past experience or, as in the
Programming by Demonstration (PbD) paradigm [20], by a human instructor
that shows to the robot how to perform the task [16, 19, 21]. Demonstrated
skills can be encoded in several ways. Dynamical systems (DS) are a promising
approach to represent demonstrated skills and plan robotic motions in real-time.
DS have been successfully used in a variety of robotic applications including
point-to-point motion planning [3,6, 14, 17], reactive motion replanning [1,2,5,7],
and learning impedance behaviors from demonstrations [18, 24].
Although DS are widely used in robotics applications, there is not much work
on incremental learning approaches for DS. Some approaches have extended the
Dynamic Movement Primitives (DMPs) [6] towards incremental learning. In [8],
authors provide a corrective demonstration to modify a part of a DMP trajectory
while keeping the rest unchanged. The works in [9, 10] propose passivity-based
2 Saveriano and Lee
Fig. 1. System overview. (Top) The user observes the original robot behavior. (Middle)
If the robot behavior does not match the requirements (it hits the red bar in the
depicted case), novel demonstrations of the task are provided. (Bottom) The robot
executes the refined behavior and avoids the obstacle.
controllers to allow a human operator to incrementally demonstrate a DMP tra-
jectory safely interacting with the robot during the execution. The limitation of
these approaches is that they rely on DMPs that are time dependent DS. As
quantitatively shown in [15], time dependent DS use task dependent heuristics
to preserve the shape of a demonstrated motion in the face of temporal or spa-
cial perturbations. On the other hand, time-independent DS naturally adapt to
motion perturbations and exhibit higher generalization capabilities.
In [12], authors propose to reshape the velocity of an autonomous (i.e. time in-
dependent) DS using a modulation matrix. The modulation matrix is parametrized
as a rotation matrix and a scalar gain. These parameters are incrementally
learned from demonstration using Gaussian processes regression [13]. The ap-
proach in [12] has the advantage to locally modify the DS dynamics, i.e. the
dynamics in regions of the state space far from the demonstrated trajectories re-
main unchanged. The main limitations of [12] are that it directly applies only to
first-order DS and to low-dimensional spaces (up to 3 dimensions where rotations
can be uniquely represented). In our previous work [4], we propose to suppress a
learned reshaping term (additive control input) using a time-dependent signal.
The reshaping term depends on less parameters than the modulation matrix
in [12] and it naturally applies to high-dimensional DS and state spaces. More-
over, generated trajectories accurately follow the demonstrated ones. However,
the time-dependence introduces practical limitations due to the hand tuning of
the time constant used to suppress the reshaping term. For instance, if a longer
trajectory is required for some initial conditions, the reshaping term can be
suppressed too early introducing deviations from the demonstrated path.
In this paper, we propose a novel parameterization of the reshaping term that
preserves the stability of the reshaped DS without introducing time-dependencies.
This is obtained by projecting the reshaping term in the subspace orthogonal
Incremental Motion Reshaping of Autonomous Dynamical Systems 3
to the gradient of a given Lyapunov function. The presented parameterization
is general and no restrictions are imposed on the order of the DS, as well as
on the dimension of the state space. Moreover, the dynamics of the original DS
are locally affected by the reshaping action, giving the possibility to learn dif-
ferent behaviors in different regions of the state space. To this end, we adopt a
kernel based (local) regression technique, namely Gaussian process regression,
to retrieve a smooth control input for each state. The control action is learned
incrementally from user demonstrations by deciding when new points are added
to the training set. As in [4, 12], a trajectory-based sparsity criterion is used to
reduce the amount of points added to the training set and reduce the computa-
tion time. The incremental learning procedure proposed in this work is shown
in Fig. 1.
The rest of the paper is organized as follows. Section 2 presents the theo-
retical background and the proposed parameterization of the reshaping term.
The incremental learning algorithm is described in Section 3. Simulations and
experiments are presented in Sec. 4. Section 5 states the conclusions and the
future extensions.
2 Orthogonal Reshaping of Dynamical Systems
In this section, we describe an approach to reshape the dynamics of a generic
DS without modifying its stability properties.
2.1 Theoretical Background
We assume that the robot’s task is encoded into a m-th order autonomous DS
(time-dependencies are omitted to simplify the notation)
p(m)=gp,p(1),...,p(m1) (1)
where pRnis the robot position (in joint or Cartesian space), p(i)is the
i-th time derivative of pand g:RnRnis, in general, a non-linear function.
The m-th order dynamics (1) can be rewritten, through the change of variables
xT= [xT
1,...,xT
m]=[pT,...,(p(m1))T], as the first-order dynamics
˙
x1=x2
···
˙
xm=g(x1,...,xm)˙
x=f(x) (2)
where xRmn is the state vector. The solution of (2) Φ(x0, t)Rmn is called
trajectory. Different initial conditions x0generate different trajectories.
A point ˆ
x:f(ˆ
x) = 0Rmn is an equilibrium point. An equilibrium is
locally asymptotically stable (LAS) if limt+Φ(x0, t) = ˆ
x,x0SRmn. If
S=Rmn,ˆ
xis globally asymptotically stable (GAS) and it is the only equilibrium
of the DS. A sufficient condition for ˆ
xto be GAS is that there exists a scalar,
4 Saveriano and Lee
continuously differentiable function of the state variables V(x)Rsatisfying
(3a)-(3c) (see, for example, [11] for further details).
V(x)0,xRmn and V(x)=0x=ˆ
x(3a)
˙
V(x)0,xRmn and ˙
V(x)=0x=ˆ
x(3b)
V(x)→ ∞ as kxk→∞(radially unbounded) (3c)
Note that, if condition (3c) is not satisfied, the equilibrium point is LAS. A
function satisfying (3a)-(3b) is called a Lyapunov function.
2.2 Reshaping Control Input
If the task consists in reaching a specific position ˆ
x(discrete movement), one
can assume that (2) has a GAS equilibrium at ˆ
xand that a Lyapunov function
V(x) is known [6,14]. Let us consider the reshaped DS in the form
˙
x=f(x) + u(x) (4)
where u(x)=[0,...,0,um(x)] Rmn is a continuous control input that satisfies
u(ˆ
x) = 0um(ˆ
x) = 0Rn(5a)
Vxu(x) = 0 Vxmum(x) = ∂V
xm
um(x) = 0,xmRn(5b)
where Vxindicates the gradient of V(x) with respect to x, i.e. Vx=∂V (x)/∂x.
Under conditions (5) the following theorem holds:
Theorem 1. A GAS equilibrium ˆ
xof (2) is also a GAS equilibrium of the
reshaped DS (4).
Proof. From (4) and (5a) it holds that f(ˆ
x)+u(ˆ
x) = 0, i.e. ˆ
xis an equilibrium of
(4). To analyze the stability of ˆ
x, let us consider V(x), the Lyapunov function for
(2), as a candidate Lyapunov function for (4). Being V(x) a Lyapunov function
for (2) it satisfies conditions (3a) and (3c) also for the reshaped DS (4). The
condition (3b) can expressed in terms of the gradient of V(x) as
˙
V(x) = Vx˙
x=∂V (x)
x1
,...,V (x)
xm(f(x) + u(x))
=∂V (x)
x1
x2+. . . +V (x)
xm
g(x) +
∂V (x)
xm
um(x) = Vxf(x)<0,x6=ˆ
x
where (∂V (x)/∂xm)um(x) = 0 by assumption (5b). ut
Corollary 1. Theorem 1 implies that ˆ
xis the only equilibrium point of (4), i.e.
f(x) + u(x)vanishes only at x=ˆ
x.
Proof. In Theorem 1 it is proved that Vx(f(x) + u(x)) vanishes only at ˆ
x.
Hence, also f(x) + u(x)vanishes only at ˆ
x.ut
Incremental Motion Reshaping of Autonomous Dynamical Systems 5
Corollary 2. Theorem 1 still holds for a LAS equilibrium, i.e. if ˆ
xis a LAS
equilibrium of (2) then ˆ
xis a LAS equilibrium of (4).
Proof. If ˆ
xis LAS, only the conditions (3a)-(3b) are satisfied xSRmn.
The proof of Theorem (1) still holds if xSRmn .ut
Theorem 1 has a clear physical interpretation for second-order dynamical
systems in the form ˙
x1=x2,˙
x2=K(ˆ
x1x1)Dx2, where Kand Dare
positive definite matrices and ˆ
x= [ˆ
xT
1,0T]Tis the equilibrium point. The GAS
of ˆ
xcan be proven through the energy-based Lyapunov function V=1
2(ˆ
x1
x1)TK(ˆ
x1x1) + 1
2xT
2x2and the La Salle’s theorem [11]. The assumptions
on umin Theorem 1 can be satisfied by choosing umorthogonal to Vx2=x2.
Hence, umis a force that does no work (orthogonal to the velocity), i.e. um
modifies the trajectory of the system but not its energy.
2.3 Control input parametrization
In order to satisfy the conditions (5) we choose the control input umin (4) as
um=(Nud=Nh(x1) (pd(x1)x1) if x6=ˆ
x
0otherwise (6)
where x1Rnis the position of the robot. The scalar gain h(x1)0 and
the desired position pd(x1)Rnare learned from demonstrations (see Sec.
3). The adopted parametrization (6) requires always n+ 1 parameters, i.e. the
position vector pdRn(where nis the Cartesian or joint space dimension)
and the scalar gain hR. For comparison, consider that the parametrization
in [12] uses a rotation and a scalar gain and that a minimal representation of
the orientation in Rnrequires at least n(n1)/2 parameters [25]. The vector ud
represents an elastic force attracting the position x1towards the desired position
pd. The matrix Nis used to project udinto the subspace orthogonal to Vxm
and it is defined as
N=kVxmk2In×n¯
VT
xm
¯
Vxm(7)
where In×nis the n-dimensional identity matrix and ¯
Vxm=Vxm/kVxmk. The
term kVxmk2guarantees a smooth convergence of umto zero. Note that the
control input um0if h(x1)0. This property is exploited in Sec. 3 to
locally modify the trajectory of the DS.
3 Learning Reshaping Terms
In this section, an approach is described to learn and online retrieve for each po-
sition x1the parameter vector λ= [h, pT
d] that parametrizes the control input
in (6). We use a local regression technique, namely Gaussian process regression
(GPR), to ensure that um0(h0) when the robot is far from the demon-
strated trajectories. This makes it possible to locally follow the demonstrated
trajectories, leaving the rest almost unchanged.
6 Saveriano and Lee
3.1 Compute Training Data
Consider that a new demonstration of a task is given as X={xt
d,1,˙
xt
d,m}T
t=1,
where xt
d,1Rnis the desired position at time tand ˙
xt
d,m Rnis the time
derivative of the last state component xt
d,m at time t. For example, if one wants
to reshape a second-order DS, then Xcontains Tpositions and Taccelerations.
The procedure to transform the demonstration into Tobservations of Λ={λt=
[λ1, . . . , λn+1]t=h, pT
dt}T
t=1 requires following steps:
I. Set {pt
d=xt
d,1}T
t=1, where xd,1are the demonstrated positions.
The gain htin (6) multiplies the position error pdx1and it is used to modulate
the control action umand to improve the overall tracking performance. The value
of htis computed by considering that ˙
xm=g(x) + umfrom (2) and (4). The
following steps are needed:
II. Create the initial state vector x0
d= [(x1
d,1)T,0T,...,0T]TRmn.
III. Compute xt
o=Φ(x0
d,(t1)δt)T
t=1, where Φis the solution of (2) with
initial condition x0
d(see Sec. 2.1) and δt is the sampling time.
IV. Compute {ut
m}T
t=1 from (6) with h= 1 and {xt
1=xt
o,1}T
t=1.
V. Set
ht=
k˙
xt
d,m g(xt
o)k
kut
mkif kut
mk>0
0 otherwise
t= 1, . . . , T (8)
Once the observations of λare computed, any local regression technique can be
applied to learn the relationship between λand the position x1of the DS to
reshape.
3.2 Gaussian Process Regression
Gaussian processes (GP) are widely used to learn input-output mappings from
observations [13]. GP models the scalar noisy process λt=f(xt
1) + R, t =
1, . . . , T with a Gaussian noise with zero mean and variance σ2
n. Therefore,
nprocesses λt
iare assumed to generate the training input X={xt
1}T
t=1 and
output Λi={λt
i}T
t=1. Given the training pairs (X,Λi) and a query point x, it
is possible to compute the joint distribution
Λi
λ
i∼ N 0,KX X +σ2
nI KxX
KXxk(x,x) (9)
where λ
iis the expected output at x. The matrix KxX={k(x,xt
1)}T
t=1,
KXx=KT
xX. Each element ij of KXX is given by {KXX }ij =k(xi,xj),
where k(,) is a user-defined covariance function. In this work, we used the
squared exponential covariance function
k(xi,xj) = σ2
kexp kxixjk2
2l+σ2
nδ(xi,xj) (10)
Incremental Motion Reshaping of Autonomous Dynamical Systems 7
k(xi,xj) is parameterized by the 3 positive parameters σ2
k,σ2
n, and l. The tunable
parameters σ2
k,σ2
n, and lcan be hand-crafted or learned from training data [13].
We decide to keep them fixed in order to perform incremental learning by simply
adding new points to the training set. It is worth noticing that the adopted
kernel function (10) guarantees that λ0for points far from the demonstrated
positions.
Predictions with a GP model are made using the conditional distribution of
λ
i|Λi, i.e.
λ
i|Λi∼ N µλ
i|Λi, σ2
λ
i|Λi(11)
where
µλ
i|Λi=KxXKXX +σ2
nI1Λi
σ2
λ
i|Λi=k(x,x)KxXKXX +σ2
nI1KXx
(12)
The mean µλ
i|Λiapproximates λ
i, while the variance σ2
λ
i|Λiplays the role of a
confidence bound. If, as in this work, a multidimensional output is considered,
one can simply train one GP for each dimension.
To reduce the computation effort due to the matrix inversion in (12), incre-
mental GP algorithms introduce criteria to sparsely represent incoming data [22].
Assuming that Tdata {xt
1, ht,pt
d}T
t=1 are already in the training set, we add a
new data point [xT+1
1, hT+1,pT+1
d] if the cost
CT+1 =kpT+1
dˆ
pT+1
dk>¯c(13)
where ˆ
pT+1
dindicates the position predicted at xT+1
1with (12) using only data
{xt
1, ht,pt
d}T
t=1 already in the training set. Similarly to [4, 12], the tunable
parameter ¯crepresents the error in approximating demonstrated positions and
it can be easily tuned. For example, ¯c= 0.2 means that position errors smaller
than 0.2 meters are acceptable. The proposed incremental reshaping approach
is summarized in Tab. 1.
4 Results
4.1 Simulation - Learning Bi-Modal Behaviors
The goal of this simulation is to illustrate the incremental nature of the proposed
reshaping approach, its ability to learn different behaviors in different regions of
the space, and the possibility to reshape high order DS. The original trajectory
is obtained by numerically integrating (δt = 0.01 s) the second-order DS
˙
x1=x2
˙
x2=10x1210x2
(14)
8 Saveriano and Lee
Table 1. Proposed reshaping approach.
Batch
Create a set of predefined tasks encoded as stable DS.
Stable DS can be designed by the user or learned
from demonstrations as in [6, 14].
Provide a Lyapunov function V(x) for each DS.
Incremental
Observe the robot’s behavior in novel scenarios.
If needed, provide a corrective demonstration, for
example by kinesthetic teaching the robot.
Learn the parameters of the control input (4), as
described in Sec. 3. Tuning parameters can be set
empirically by simulating the reshaped DS.
Repeat until the refined behavior is satisfactory.
where x1= [x, y]TR2is the position and x2the velocity. The system (14) has
a GAS equilibrium at ˆ
x=0R4and Lyapunov function V(x) = 1
2(xT
1x1+
xT
2x2).
Local demonstrations are drawn from different Gaussian distributions, as de-
scribed in Tab. 2, to obtain different bi-modal behaviors. A total of four demon-
strations are used in each case, i.e. two (red and green crosses in Fig. 2) for the
behavior in the region R+where x > 0, two (magenta and blue crosses in Fig.
2) for the region Rwhere x < 0. As shown in Fig. 2, the original DS position
trajectory (black solid line) is incrementally adapted to follow the demonstrated
positions and different behaviors are effectively learned in R+and R. Totally
four simulations are conducted and shown in Fig. 2. In all the presented cases,
the DS is successfully reshaped to follow the demonstrated trajectories. The pro-
posed approach locally modifies the DS, in fact demonstrations in R+, being far
from R, do not affect the behavior in R(and vice versa). The equilibrium
position ˆ
x1=0R2is always reached, as expected from Theorem 1. Results
are obtained with noise variance σ2
n= 0.1, signal variance σ2
k= 1, length scale
l= 0.4, and threshold ¯c= 0.02 m.
4.2 Experiments
The effectiveness of the proposed approach is demonstrated with two experi-
ments on a KUKA LWR IV+ 7 DoF manipulator. In both experiments, novel
demonstrations of the desired position are provided to the robot by kinesthetic
teaching. To guarantee a safe physical guidance, the task is interrupted and
the robot is put in the gravity compensation mode as soon as the user touches
the robot. The external torques estimation provided by the fast research inter-
face [23] is used to detect physical contacts.
Incremental Motion Reshaping of Autonomous Dynamical Systems 9
Table 2. Demonstrations used for the four simulations in Fig. 2.
Figure Demonstrations in RDemonstrations in R+
2(a) x[2,2.5] x[2,2.5]
y= 0.1N(2,0.03) y= 0.1N(2,0.03)
2(b) x[2,2.5] x[2,2.5]
y=0.1N(2,0.03) y= 0.1N(2,0.03)
2(c) x[2,2.5] x[2,2.5]
y= 0.1N(2,0.03) y=0.1N(2,0.03)
2(d) x[2,2.5] x[2,2.5]
y=0.1N(2,0.03) y=0.1N(2,0.03)
0.12
0.1
0.08
0.06
0.04
0.02
0
-3 -2 -1 3
2
1
0
original
trajectory
x [m]
y [m]
reshaped
trajectories
(a)
0.1
0.05
0
-0.1
-0.05
-3 -2 -1 3
2
1
0
x [m]
original
trajectory
reshaped
trajectories
(b)
(c)
original
trajectory
reshaped
trajectories
-3 -2 -1 3
2
1
0
x [m]
-0.12
-0.1
-0.08
-0.06
-0.04
-0.02
0
(d)
Fig. 2. Different bi-modal behaviors obtained by reshaping the same dynamical sys-
tem. Red and magenta dashed lines are the reshaped trajectories after providing two
demonstrations, one (red crosses) for R+and one (magenta crosses) for R. Blue and
green solid lines are the reshaped trajectories after providing four demonstrations.
End-effector Collision Avoidance This experiment shows the ability of the
proposed reshaping approach to learn different behaviors in different regions of
the space and the possibility to reshape non-linear DS. The task consists in
reaching the goal position ˆ
x1= [ 0.52,0,0.02]Tm with the robot’s end-effector
while avoiding a box (see Fig. 3(d)) of size 7 ×7×23 cm. Two boxes are placed
10 Saveriano and Lee
in the scene in different positions, one in the region R+where y > 0 (Fig. 3(d)),
one in the region Rwhere y < 0 (Fig. 3(e)). Hence, the robot has to learn a
bi-modal behavior to avoid collisions in R+and R.
The original position trajectory is obtained by numerically integrating (δt =
0.005 s) the first-order and non-linear DS ˙
x1=f(x1ˆ
x1), where x1= [x, y, z]T
R3is the end-effector position and ˙
x1R3is the end-effector linear velocity.
The orientation is kept fixed. The original DS is learned from demonstrations
by using the approach in [14]. The Lyapunov function for the original DS is
V=1
2(x1ˆ
x1)T(x1ˆ
x1) [14]. The original end-effector trajectories are shown
in Fig. 3(a)–(c) (black solid lines).
Following the original trajectory generated with initial position x1(0) = [
0.52,0.5,0.02]Tm (or x1(0) = [0.52,0.5,0.02]Tm), the robot hits the box. To
prevent this, two partial demonstrations (one in R+and one R) are provided
to show to the robot how to avoid the obstacles (brown solid lines in Fig. 3(a)–
(c)). The original DS position trajectories (black solid lines in Fig. 3(a)–(c))
are incrementally adapted to follow the demonstrated positions and different
avoiding behaviors are effectively learned in R+and R.
The proposed approach locally modifies the DS, in fact demonstrations in
R+, being far from R, do not affect the behavior in R(and vice versa). The
equilibrium position ˆ
x1= [ 0.52,0,0.02]Tm is always reached, as stated by
Theorem 1. Figure 3 also shows the learned behaviors (green in R+and blue
in Rsolid lines) for different initial positions in a 3D view (Fig. 3(a)) and in
the xz plane (Fig. 3(b) and 3(c)). In all cases, the robot is able to achieve the
task. Snapshots of the learned bi-modal behavior are depicted in Fig. 3(d) and
3(e). Results are obtained with noise variance σ2
n= 0.1, signal variance σ2
k= 1,
length scale l= 0.001, and the threshold ¯c= 0.04 m. With the adopted ¯conly
106 points over 798 are added to the GP.
Joint Space Collision Avoidance This experiment shows the scalability of
the proposed approach to high dimensional spaces and its ability to reshape high
order DS. The task is a point-to-point motion in the joint space from x1(0) =
[35,55,15,65,15,50,90]Tdeg to ˆ
x1= [60,30,30,70,15,85,15]Tdeg. The
original joint position trajectory is obtained by numerically integrating (δt =
0.005 s) the second-order DS ˙
x1=x2,˙
x2= 2(ˆ
x1x1)22x2, where x1=
[θ1, . . . , θ7]TR7are the joint angles and x2R7the joint velocities. The
system has a GAS equilibrium at ˆ
x= [ˆ
xT
1,0T]TR14 and Lyapunov function
V=1
2(ˆ
x1x1)T(ˆ
x1x1) + 1
2xT
2x2. The original joint angle trajectories are
shown in Fig. 4 (black solid lines).
As shown in Fig. 1, following the original trajectory the robot hits an un-
foreseen obstacle (the red bar in Fig. 1). A kinesthetic demonstration is then
provided (red lines in Fig. 4) to avoid the collision. With the reshaped trajec-
tory (blue lines in Fig. 4) the robot is able to avoid the obstacle (Fig. 1) and
to reach the desired goal ˆ
x1. Results are obtained with noise variance σ2
n= 0.1,
signal variance σ2
k= 1, length scale l= 0.01, and the threshold ¯c= 15 deg. With
the adopted ¯conly 27 points over 178 are added to the GP.
Incremental Motion Reshaping of Autonomous Dynamical Systems 11
0.1
0.09
0
0.01
0.02
0.03
0.04
0.05
0.06
0.07
0.08
0.5 0.4 0.3 0.2 0.1 0-0.1 -0.2 -0.3 -0.4 -0.5
-0.58
-0.56
-0.54
-0.52
-0.5
-0.48
-0.46
-0.44
-0.42
-0.4
y [m]
z [m]
x [m]
End-effector trajectories
goal pos.
initial pos.
original traj.
reshaped traj.
reshaped traj.
demonstration
obstacles
(a) 3D view.
0.1
0.09
0
0.01
0.02
0.03
0.04
0.05
0.06
0.07
0.08
z [m]
-0.4 x [m]
-0.44-0.42 -0.46 -0.48 -0.5 -0.52 -0.54 -0.56 -0.58
(b) XZ view in R+.
0.1
0.09
0
0.01
0.02
0.03
0.04
0.05
0.06
0.07
0.08
z [m]
-0.4
x [m]
-0.44-0.42
-0.46
-0.48 -0.5
-0.52-0.54-0.56-0.58
(c) XZ view in R.
(d) Reshaped trajectory starting at x1(0) ∈ R+.
(e) Reshaped trajectory starting at x1(0) ∈ R.
Fig. 3. Results of the end-effector collision avoidance experiment.
4.3 Discussion
The proposed approach works for high order DS, as underlined in Sec. 2 and
demonstrated in Sec. 4.2. Being robot manipulators dynamics described by
second-order DS, a second-order DS is sufficient to generate dynamically feasible
12 Saveriano and Lee
50
0
-50
-100
Joint angles trajectories
60
50
40
30
30
25
20
15 -100
-80
-60
-10
-20
0
40
60
80
100
50
0
100
0510 15 20
time [s]
Fig. 4. Original joint angles trajectories (black lines), the provided demonstration (red
lines) and reshaped joint angles trajectories (blue lines) for the joint space collision
avoidance experiment.
trajectories. For this reason, we show results for DS up to the second order. The
adopted control law in (6) pushes the robot position towards the demonstrated
position, without considering desired velocities or accelerations. We adopt this
solution because, in the majority of the cases, a user is interested in reconfig-
uring the robot and (s)he can hardly show a desired velocity (or acceleration)
behavior through kinesthetic teaching.
It must be noted that the proposed control law (6) does not always guarantee
good tracking of the demonstrated trajectories, as shown in Fig. 4. In general, to
have good tracking performance, different controllers have to be designed for dif-
ferent DS [11]. Nevertheless, in this work we do not focus on accurately tracking
the demonstrated trajectories, but we want to modify the robot’s behavior until
the task is correctly executed. The joint angle trajectories in Fig. 4 guarantee
the correct execution of the task, i.e. the robot converges to the desired joint
position while avoiding the obstacle. The loss of accuracy also depends on the
orthogonality constraint in (5b) between the gradient of the Lyapunov function
and the control input. This constrain allows only motions perpendicular to the
gradient to be executed, which limits the control capabilities and increases the
number of demonstrations needed in order to obtain the satisfactory behavior. In
principle, it is possible to relax the constraint (5b) by requiring that Vxu(x)0.
The design of a control input the satisfies Vxu(x)0 is left as future work.
Figure 4 shows an overshoots in the resulting position trajectory (see, for
instance, the angle θ6). To better understand this behavior, consider that we are
controlling a spring-damper (linear) DS with a proportional controller (with a
non-linear gain). In case the resulting closed-loop system is not critically damped,
Incremental Motion Reshaping of Autonomous Dynamical Systems 13
the retrieved trajectory overshoots the goal position. For the experiment in Sec.
4.2, adding a damping control action (PD-like controller) would solve the over-
shoot problem. However, there is no guarantee that a generic non-linear DS does
not overshoot under a PD-like control action. Moreover, adding another term
to (6) will increase the number of parameters to learn. Therefore, we use a pro-
portional controller in this work and leave the learning of more sophisticated
controllers as a future extension.
5 Conclusions and Future Work
We presented a novel approach to incrementally modify the position trajectory of
a generic dynamical system, useful to on-line adapt predefined tasks to different
scenarios. Compared to state-of-the-art approaches, our method works also for
high-order dynamical systems, preserves the time-independence of the DS, and
does not affect the stability properties of the reshaped dynamical system, as
shown in the conducted Lyapunov-based stability analysis.
A control law is proposed that locally modifies the trajectory of the dynamical
system to follow a desired position. Desired positions, as well as the control gain,
are learned from demonstrations and retrieved on-line using Gaussian process
regression. The procedure is incremental, meaning that the user can add novel
demonstrations until the learned behavior is not satisfactory. Due to the local
nature of the reshaping control input, different behaviors can be learned and
executed in different regions of the space. Simulations and experiments show
the effectiveness of the proposed approach in reshaping non-linear, high-order
dynamical systems, and its scalability to high dimensional spaces (up to R14).
Our approach applies to dynamical systems with a LAS or a GAS equilibrium
point. Nevertheless, DS that converges towards periodic orbits (limit cycles) have
been used in robotic applications to generate periodic behaviors [6]. Compared
to static equilibria, limit cycles stability has a different characterizations in terms
of Lyapunov analysis. Our next research will focus on considering incremental
reshaping of periodic motions while preserving their stability properties.
References
1. Saveriano, M., Lee, D.: Point Cloud based Dynamical System Modulation for Re-
active Avoidance of Convex and Concave Obstacles. International Conference on
Intelligent Robots and Systems, pp. 5380–5387 (2013).
2. Saveriano, M., Lee, D.: Distance based Dynamical System Modulation for Reactive
Avoidance of Moving Obstacles. International Conference on Robotics and Automa-
tion, pp. 5618–5623 (2014).
3. Blocher, C., Saveriano, M., Lee, D.: Learning Stable Dynamical Systems using Con-
traction Theory. Ubiquitous Robots and Ambient Intelligence, pp. 124–129 (2017).
4. Saveriano, M., Lee, D.: Incremental Skill Learning of Stable Dynamical Systems.
International Conference on Intelligent Robots and Systems, pp. 6574–6581 (2018).
5. Saveriano, M., Hirt, F., Lee, D.: Human-aware motion reshaping using dynamical
systems. Pattern Recognition Letters 99, 96–104 (2017).
14 Saveriano and Lee
6. Ijspeert, A., Nakanishi, J., Pastor, P., Hoffmann, H., Schaal, S.: Dynamical Move-
ment Primitives: learning attractor models for motor behaviors. Neural Computa-
tion 25:2, 328–373 (2013).
7. Khansari-Zadeh, S. M., Billard, A.: A dynamical system approach to realtime ob-
stacle avoidance. Autonomous Robots 32:4, 433–454 (2012).
8. Karlsson, M., Robertsson, A., Johansson, R.: Autonomous interpretation of demon-
strations for modification of dynamical movement primitives. International Confer-
ence on Robotics and Automation, pp. 316–321 (2017).
9. Talignani Landi, C., Ferraguti, F., Fantuzzi, C., Secchi, C.: A passivity-based strat-
egy for coaching in human–robot interaction. International Conference on Robotics
and Automation, pp. 3279–3284 (2018).
10. Kastritsi, T., Dimeas, F., Doulgeri, Z.: Progressive Automation with DMP Syn-
chronization and Variable Stiffness Control. Robotics and Automation Letters 3:4,
3279–3284 (2018).
11. Slotine, J. J. E., Li, W.: Applied nonlinear control. Prentice-Hall (1991).
12. Kronander, K., Khansari-Zadeh, S. M., Billard, A.: Incremental Motion Learning
with Locally Modulated Dynamical Systems. Robotics and Autonomous Systems
70, pp. 52–62 (2015).
13. Rasmussen, C. E., Williams, C. K. I.: Incremental Gaussian processes for machine
learning. MIT Press (2006).
14. Khansari-Zadeh, S. M., Billard, A.: Learning Stable Non-Linear Dynamical Sys-
tems with Gaussian Mixture Models. Transactions on Robotics 27:5, 943–957 (2011).
15. Gribovskaya, E., Khansari-Zadeh, S. M., Billard, A.: Learning Non-linear Multi-
variate Dynamics of Motion in Robotic Manipulators. The International Journal of
Robotics Research 30:1, 80—117 (2011)
16. Saveriano, M., An, S., Lee, D.: Incremental Kinesthetic Teaching of End-Effector
and Null-Space Motion Primitives. International Conference on Robotics and Au-
tomation, pp. 3570–3575 (2015).
17. Saveriano, M., Franzel, F., Lee, D.: Merging position and orientation motion primi-
tives. International Conference on Robotics and Automation, pp. 7041–7047 (2019).
18. Saveriano, M., Lee, D.: Learning Motion and Impedance Behaviors from Human
Demonstrations. International Conference on Ubiquitous Robots and Ambient In-
telligence, pp. 368–373 (2014).
19. Lee, D., Ott, C.: Incremental Kinesthetic Teaching of Motion Primitives Using the
Motion Refinement Tube. Autonomous Robots 31:2, 115–131 (2011).
20. Billard, A., Calinon, S., Dillmann, R., Schaal, S.: Robot Programming by Demon-
stration. Springer Handbook of Robotics, pp. 1371–1394 (2008).
21. Calinon, S., Guenter, F., Billard, A.: On Learning, Representing, and Generalizing
a Task in a Humanoid Robot. Transactions on Systems, Man, and Cybernetics, Part
B: Cybernetics 37:2, 286–298 (2007).
22. Csat´o, L.: Gaussian Processes - Iterative Sparse Approximations. PhD Disserta-
tion, Aston University (2002).
23. Schreiber, G., Stemmer, A., Bischoff, R.: The Fast Research Interface for the
KUKA Lightweight Robot. ICRA Workshop on Innovative Robot Control Archi-
tectures for Demanding (Research) Applications - How to Modify and Enhance
Commercial Controllers, 15–21 (2010).
24. Calinon, S., Sardellitti, I., Caldwell, D.: The Learning-based control strategy for
safe human-robot interaction exploiting task and robot redundancies. International
Conference on Intelligent Robots and Systems, 249–254 (2010).
25. Mortari, D.: On the rigid rotation concept in n-dimensional spaces. Journal of the
Astronautical Sciences 49:3, 401–420 (2001).
ResearchGate has not been able to resolve any citations for this publication.
Full-text available
Conference Paper
In this paper, we focus on generating complex robotic trajectories by merging sequential motion primitives. A robotic trajectory is a time series of positions and orientations ending at a desired target. Hence, we first discuss the generation of converging pose trajectories via dynamical systems, providing a rigorous stability analysis. Then, we present approaches to merge motion primitives which represent both the position and the orientation part of the motion. Developed approaches preserve the shape of each learned movement and allow for continuous transitions among succeeding motion primitives. Presented methodologies are theoretically described and experimentally evaluated, showing that it is possible to generate a smooth pose trajectory out of multiple motion primitives.
Full-text available
Conference Paper
Efficient skill acquisition, representation, and on-line adaptation to different scenarios has become of fundamental importance for assistive robotic applications. In the past decade, dynamical systems (DS) have arisen as a flexible and robust tool to represent learned skills and to generate motion trajectories. This work presents a novel approach to incrementally modify the dynamics of a generic autonomous DS when new demonstrations of a task are provided. A control input is learned from demonstrations to modify the trajectory of the system while preserving the stability properties of the reshaped DS. Learning is performed incrementally through Gaussian process regression, increasing the robot's knowledge of the skill every time a new demonstration is provided. The effectiveness of the proposed approach is demonstrated with experiments on a publicly available dataset of complex motions.
Full-text available
Article
Progressive automation is a method that allows the operator to demonstrate a task a few times until the robot gradually learns to execute it autonomously. In this paper we combine a novel structure of Dynamic Movement Primitives with variable stiffness control, to allow synchronization with the demonstrated motion during the operator's intervention. This structure enables the DMP to speed up or slow down depending on the human demonstration. In addition, we present a variable stiffness controller to change the role of the robot between follower and leader based on the tracking error of the robot from the proposed DMP and on the guidance forces. The proposed variable stiffness controller is proved to be passive using energy tanks. The effectiveness of the proposed method is demonstrated experimentally.
Full-text available
Article
This paper discusses the learning of robot point-to-point motions via non-linear dynamical systems and Gaus-sian Mixture Regression (GMR). The novelty of the proposed approach consists in guaranteeing the stability of a learned dynamical system via Contraction theory. A contraction analysis is performed to derive sufficient conditions for the global stability of a dynamical system represented by GMR. The results of this analysis are exploited to automatically compute a control input which stabilizes the learned system on-line. Simple and effective solutions are proposed to generate motion trajectories close to the demonstrated ones, without affecting the stability of the overall system. The proposed approach is evaluated on a public benchmark of point-to-point motions and compared with state-of-the-art algorithms based on Lyapunov stability theory.
Full-text available
Article
In this work, we present a real-time approach for human-aware motion replanning using a two-level hierarchical architecture. The lower level leverages stable dynamical systems to generate motor commands and to online reshape the robot trajectories. The reshaping strategy modifies the velocity of the robot to match three requirements: i) to increase the human safety in case of close interaction with the robot, ii) to guarantee the correct task execution in case of unforeseen obstacles (including the human), and iii) to replan online the current task taking into account the human behavior. The lower level has to execute all needed computations in real-time. To this end, we also propose a novel approach that leverages depth space structure and parallel programming to rapidly and accurately estimate the robot-obstacle distances. The higher level of the architecture monitors the human and provides to the lower level information about the human status. The proposed approach has been tested in a human robot interaction scenario, showing promising results in terms of safe human-robot coexistence.
Conference Paper
This paper discusses the learning of robot pointto- point motions via non-linear dynamical systems and Gaussian Mixture Regression (GMR). The novelty of the proposed approach consists in guaranteeing the stability of a learned dynamical system via Contraction theory. A contraction analysis is performed to derive sufficient conditions for the global stability of a dynamical system represented by GMR. The results of this analysis are exploited to automatically compute a control input which stabilizes the learned system on-line. Simple and effective solutions are proposed to generate motion trajectories close to the demonstrated ones, without affecting the stability of the overall system. The proposed approach is evaluated on a public benchmark of point-to-point motions and compared with state-of-the-art algorithms based on Lyapunov stability theory.
Conference Paper
The KUKA lightweight robot (LWR) provides many unique features for robotic researchers. To give full access to these features, a new interface was developed that gives direct low-level real-time access to the KUKA robot controller (KRC) at high rates of up to 1 kHz. On the other hand, all industrial- strength features, like teaching, motion script features, fieldbus I/O and safety are provided. Using standard UDP socket technology, the user is not limited to one specific runtime system. This paper describes the capabilities of the interface, the practical realization within the LWR control architecture and first applications of the interface.