Content uploaded by Matteo Saveriano

Author content

All content in this area was uploaded by Matteo Saveriano on Mar 25, 2020

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 modiﬁes the trajectories of an autonomous dynamical

system without aﬀecting 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 eﬀec-

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 reﬁned 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

ﬁrst-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 aﬀected by the reshaping action, giving the possibility to learn dif-

ferent behaviors in diﬀerent 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(m−1) (1)

where p∈Rnis the robot position (in joint or Cartesian space), p(i)is the

i-th time derivative of pand g:Rn→Rnis, 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(m−1))T], as the ﬁrst-order dynamics

˙

x1=x2

···

˙

xm=g(x1,...,xm)−→ ˙

x=f(x) (2)

where x∈Rmn is the state vector. The solution of (2) Φ(x0, t)∈Rmn is called

trajectory. Diﬀerent initial conditions x0generate diﬀerent trajectories.

A point ˆ

x:f(ˆ

x) = 0∈Rmn is an equilibrium point. An equilibrium is

locally asymptotically stable (LAS) if limt→+∞Φ(x0, t) = ˆ

x,∀x0∈S⊂Rmn. If

S=Rmn,ˆ

xis globally asymptotically stable (GAS) and it is the only equilibrium

of the DS. A suﬃcient condition for ˆ

xto be GAS is that there exists a scalar,

4 Saveriano and Lee

continuously diﬀerentiable function of the state variables V(x)∈Rsatisfying

(3a)-(3c) (see, for example, [11] for further details).

V(x)≥0,∀x∈Rmn and V(x)=0⇐⇒ x=ˆ

x(3a)

˙

V(x)≤0,∀x∈Rmn and ˙

V(x)=0⇐⇒ x=ˆ

x(3b)

V(x)→ ∞ as kxk→∞(radially unbounded) (3c)

Note that, if condition (3c) is not satisﬁed, 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 speciﬁc 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 satisﬁes

u(ˆ

x) = 0⇐⇒ um(ˆ

x) = 0∈Rn(5a)

Vxu(x) = 0 ⇐⇒ Vxmum(x) = ∂V

∂xm

um(x) = 0,∀xm∈Rn(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 satisﬁes 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 satisﬁed ∀x∈S⊂Rmn.

The proof of Theorem (1) still holds if x∈S⊂Rmn .ut

Theorem 1 has a clear physical interpretation for second-order dynamical

systems in the form ˙

x1=x2,˙

x2=K(ˆ

x1−x1)−Dx2, where Kand Dare

positive deﬁnite 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(ˆ

x1−x1) + 1

2xT

2x2and the La Salle’s theorem [11]. The assumptions

on umin Theorem 1 can be satisﬁed by choosing umorthogonal to Vx2=x2.

Hence, umis a force that does no work (orthogonal to the velocity), i.e. um

modiﬁes 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 x1∈Rnis 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 pd∈Rn(where nis the Cartesian or joint space dimension)

and the scalar gain h∈R. 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(n−1)/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 deﬁned 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 um→0if 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 um→0(h→0) 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,1∈Rnis 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 pd−x1and 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]T∈Rmn.

III. Compute xt

o=Φ(x0

d,(t−1)δ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 Kx∗X

KXx∗k(x∗,x∗) (9)

where λ∗

iis the expected output at x∗. The matrix Kx∗X={k(x∗,xt

1)}T

t=1,

KXx∗=KT

x∗X. Each element ij of KXX is given by {KXX }ij =k(xi,xj),

where k(•,•) is a user-deﬁned covariance function. In this work, we used the

squared exponential covariance function

k(xi,xj) = σ2

kexp −kxi−xjk2

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 ﬁxed 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=Kx∗XKXX +σ2

nI−1Λi

σ2

λ∗

i|Λi=k(x∗,x∗)−Kx∗XKXX +σ2

nI−1KXx∗

(12)

The mean µλ∗

i|Λiapproximates λ∗

i, while the variance σ2

λ∗

i|Λiplays the role of a

conﬁdence bound. If, as in this work, a multidimensional output is considered,

one can simply train one GP for each dimension.

To reduce the computation eﬀort 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 diﬀerent behaviors in diﬀerent 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=−10x1−2√10x2

(14)

8 Saveriano and Lee

Table 1. Proposed reshaping approach.

Batch

Create a set of predeﬁned 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 reﬁned behavior is satisfactory.

where x1= [x, y]T∈R2is the position and x2the velocity. The system (14) has

a GAS equilibrium at ˆ

x=0∈R4and Lyapunov function V(x) = 1

2(xT

1x1+

xT

2x2).

Local demonstrations are drawn from diﬀerent Gaussian distributions, as de-

scribed in Tab. 2, to obtain diﬀerent 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 R−where x < 0. As shown in Fig. 2, the original DS position

trajectory (black solid line) is incrementally adapted to follow the demonstrated

positions and diﬀerent behaviors are eﬀectively 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 modiﬁes the DS, in fact demonstrations in R+, being far

from R−, do not aﬀect the behavior in R−(and vice versa). The equilibrium

position ˆ

x1=0∈R2is 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 eﬀectiveness 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 R−Demonstrations 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)

-3 -2 -1 3

2

1

0

x [m]

0.1

0.05

0

-0.1

-0.05

y [m]

original

trajectory

reshaped

trajectories

(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. Diﬀerent 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-eﬀector Collision Avoidance This experiment shows the ability of the

proposed reshaping approach to learn diﬀerent behaviors in diﬀerent 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-eﬀector

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 diﬀerent positions, one in the region R+where y > 0 (Fig. 3(d)),

one in the region R−where 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 ﬁrst-order and non-linear DS ˙

x1=f(x1−ˆ

x1), where x1= [x, y, z]T∈

R3is the end-eﬀector position and ˙

x1∈R3is the end-eﬀector linear velocity.

The orientation is kept ﬁxed. 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-eﬀector 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 diﬀerent

avoiding behaviors are eﬀectively learned in R+and R−.

The proposed approach locally modiﬁes the DS, in fact demonstrations in

R+, being far from R−, do not aﬀect 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 R−solid lines) for diﬀerent 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(ˆ

x1−x1)−2√2x2, where x1=

[θ1, . . . , θ7]T∈R7are the joint angles and x2∈R7the joint velocities. The

system has a GAS equilibrium at ˆ

x= [ˆ

xT

1,0T]T∈R14 and Lyapunov function

V=1

2(ˆ

x1−x1)T(ˆ

x1−x1) + 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-eﬀector 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 suﬃcient 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 reconﬁg-

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, diﬀerent 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 satisﬁes 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 predeﬁned tasks to diﬀerent

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 aﬀect the stability properties of the reshaped dynamical system, as

shown in the conducted Lyapunov-based stability analysis.

A control law is proposed that locally modiﬁes 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, diﬀerent behaviors can be learned and

executed in diﬀerent regions of the space. Simulations and experiments show

the eﬀectiveness 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 diﬀerent 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., Hoﬀmann, 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 modiﬁcation 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 Stiﬀness 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-Eﬀector

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 Reﬁnement 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., Bischoﬀ, 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).