The 11th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI 2014)Nov. 12 – 15, 2014 at Double Tree Hotel by Hilton, Kuala Lumpur, Malaysia

Learning Motion and Impedance Behaviors from Human Demonstrations

Matteo Saveriano1 and Dongheui Lee1

1Chair of Automatic Control Engineering, Technical University of Munich, Munich, Germany(Tel : +49-89-289-268840, +49-89-289-25780; E-mail: matteo.saveriano@tum.de, dhlee@tum.de)

Abstract - Human-robot skill transfer has been deeplyinvestigated from a kinematic point of view, generatingvarious approaches to increase the robot knowledge in asimple and compact way. Nevertheless, social roboticsapplications require a close and active interaction withhumans in a safe and natural manner. Torque controlledrobots, with their variable impedance capabilities, seem aviable option toward a safe and profitable human-robotinteraction. In this paper, an approach is proposed tosimultaneously learn motion and impedance behaviorsfrom tasks demonstrations. Kinematic aspects of the taskare represented in a statistical way, while the variabil-ity along the demonstrations is used to define a variableimpedance behavior. The effectiveness of our approachis validated with simulations on real and synthetic data.

Keywords - Learning from Demonstrations, variableimpedance control, state-dependent behavior.

1. IntroductionIn real applications, the robot is required to execute

complex tasks and to adapt its behavior to guarantee asafe interaction with humans and dynamic environments.Skills acquisition and their compact representation asmotion primitives, as well as on-line adaptation of thoseskills to new scenarios [1], are of importance both in in-dustrial and service contexts.

Leaning from Demonstration (LfD) is a powerful toolto teach skills in a simple and natural way. In LfD an ex-pert user provides some demonstrations of a task, phys-ically guiding the robot or executing the task himself.Hence, also users that are not familiar with robot pro-gramming can easily teach new skills. Among the others,approaches have been developed to learn stable dynam-ical systems (DS) from demonstration [2, 3]. DS drivenrobots are guaranteed to reach the desired position andcan react in real-time to external perturbations, such aschanges in the goal position or unforeseen obstacles [4-8].

Nevertheless, in dynamic and highly populated envi-ronments, sudden perturbations are expected and a stiffcontroller will generate high forces, making the interac-tion dangerous. Impedance control [9] is a suitable ap-proach to control the dynamic response of the robot. Thegoal of impedance control is to regulate the mechanicalimpedance of the manipulator, i.e. making the robot end-effector acting as a mass-spring-damping system. Chang-ing the impedance parameters during the task executionwill generate richer behaviors. Hence, the new problemarises of how a robot can learn impedance behaviors.

Impedance in humans has been studied to understand

the roles of the different body parts, such as muscles, ten-dons, brain and spinal cord, in modulating impedanceduring the interactions with the environment. A devicecapable to measure human stiffness is used in [10] for ex-amining the so-called equilibrium-point control hypoth-esis during multijoint arm movements. In [11] a simi-lar setup is adopted to show that humans learn an opti-mal variable impedance to stabilize intrinsically unstablemovements. Bio-inspired algorithms have also been de-veloped to mimic human impedance with robots [12].

The significant differences in kinematics and dynam-ics make hard for robots to reproduce humans impedance.Alternative solutions have been developed in the LfDframework. The main idea is to learn the stiffness fromthe variability of human demonstrations, i.e. setting highstiffness values (accurate tracking) where the demonstra-tions are similar and low values (compliance) where thedemonstrations exhibit high variability. Obviously, it isimplicitly assumed that the user demonstrates the taskwith high variability where he wants the robot to be com-pliant and low variability where the robot must be stiff.

This idea is applied in [13] to learn the time-varying proportional gains (stiffness) of a mixture of Kproportional-derivative systems used to represent the de-sired acceleration trajectory. The time-dependent stiff-ness is inversely proportional to the variance along thedemonstrations. In [14] collaborative impedance robotbehaviors are learned. The core idea is to virtually con-nect the robot’s end-effector to a set of virtual springsdriving the robot behavior. The time-varying stiffnessmatrices are learned using a weighted least square ap-proach. Finally, in [15] a technique to modify robot’sstiffness online is presented. The robot executes a taskwith high, constant stiffness in each time instant and theuser can only decrease the stiffness by shaking the robot.

The result of the previous approaches is a time-varying stiffness. For many tasks, a state-varying, time-independent impedance behavior is desirable, being morerobust to delays in the execution of the task. Indeed,a time-varying stiffness can fail to provide adequateimpedance behaviors at the right time and in the rightplace when the execution time changes. Our approachconsists in learning a state-dependent stiffness exploringthe variability of human demonstrations. Firstly, the kine-matic aspects (position and velocity) of the task are en-coded in a time-invariant DS as in [2]. Secondly, the vari-ability of the demonstrations, captured by Gaussian mix-ture models [16], generates a continuous, state-dependentstiffness matrix. A Cartesian force is then computed thatdrives the robot to complete the task.

The rest of the paper is organized as follows. Section

2 describes the adopted control strategy and the proposedimpedance learning algorithm. In Sec. 3 we present thesimulation results. Section 4 states the conclusions andthe future works.

2. Proposed ApproachIn this section we present our approach for learning

state-dependent motion and impedance behaviors fromhuman demonstration. Firstly, we underline some impor-tant concepts concerning the SEDS algorithm [2], used totrain a motion primitive in the form of a globally asymp-totically stable (GAS) DS. Secondly, we explain how thestiffness is estimated from Gaussian regression. Finally,we discuss the control law used to execute the desiredbehavior.

2.1 Learning stable motion primitivesWe assume that the set of N demonstrations

{xt,n, xt,n}T ,Nt=0,n=1, where x ∈ Rd is the position and

x ∈ Rd the velocity, are instances of a first order, nonlin-ear DS in the form:

x = f(x) + η , (1)

where f(x) : Rd → Rd is a nonlinear continuous func-tion with a unique equilibrium point in x∗ = f(x∗) = 0,and η ∈ Rd is a zero mean Gaussian noise. Having thenoise distribution a zero mean it is possible to use regres-sion to estimate the noise-free model x = f(x).

To estimate the noise-free DS, a probabilistic frame-work is used that models f as a finite mixture of Gaus-sian functions. Therefore, the nonlinear function f isparametrized by the priors P(k) = πk, the means µk andthe covariance matrices Σk of the k = 1, . . . ,K Gaus-sian functions. The means and covariance matrices aredefined by:

µk =

[µk

x

µkx

], Σk =

[Σk

x Σkxx

Σkxx Σk

x

]. (2)

A probability density functionP(xt,n, xt,n;Θ), in theform of a mixture of Gaussian components, is associatedto each point in the demonstrated trajectories:

P(xt,n, xt,n|Θ) =

K∑k=1

πkN (xt,n, xt,n|µk,Σk) , (3)

where Θ = {π1,µ1,Σ1, . . . , πK ,µK ,ΣK} are theprior, the mean and the covariance matrix of each com-ponent. Taking the posterior mean probability P(x|x) asan estimation of f yields [16]:

x = f(x) =

K∑k=1

hk(x)(Akx+ bk) , (4)

where:

Ak = Σkxx(Σ

kx)

−1

bk = µkx −A

kµkx

hk(x) =πkN (x|µk

x,Σkx)∑K

i=1 πiN (x|µi

x,Σix).

(5)

The nonlinear function f is then expressed as a non-linear sum of linear dynamical systems. To guarantee thatthe DS in Eq. (4) has a GAS equilibrium in x∗, the pa-rameters Θ can be estimated solving the following opti-mization problem [2]:

minΘ

J(Θ) = −N∑

n=1

T∑t=0

logP(xt,n, xt,n|Θ)

subject to

bk = −Akx∗

Ak negative definite

Σk positive definite

0 ≤ πk ≤ 1

K∑k=1

πk = 1

∀k ∈ 1, . . . ,K (6)

where P(xt,n, xt,n|Θ) is defined in Eq. (3).

2.2 Learning variable stiffnessThe probabilistic framework described in Sec. 2.1can

be also used to retrieve an estimation of the stiffness foreach position, following the principle that the robot mustbe stiff where demonstrations are similar and compliantotherwise. The variability of the demonstrations, at theposition level1, is captured by the mixture regression inthe covariance matrices Σk

x (Eq. (2)). Given the currentrobot position x we calculate the covariance matrix:

Σx =

K∑k=1

(hk(x))2Σkx , (7)

where hk(x) is defined in Eq. (5). In practise, we weightthe contribution of each matrix using the responsibilityhk(x) that each Gaussian has in x. The computed covari-ance matrix is symmetric and positive definite. Hence, weare allowed to calculate its eigenvalues decomposition:

Σx = EΛE−1 , (8)

where E is the matrix of the eigenvectors (principal di-rections) and Λ = diag(λ1, . . . , λd) is the diagonal ma-trix of the eigenvalues.

The root square of the eigenvalues of Σx, σi =√λi,

represents the variability (standard deviation) of the dataalong each direction. We propose to construct the stiff-ness saving the principal directions of Σx, choosing theeigenvalues inversely proportional to σi. The stiffnessmatrix can be written as:

K = ESE−1 , (9)

1The variability at the velocity level, or between position and velocityis not taken into account. We claim, in fact, that the user can hardly takeinto account these kinds of variability while he performs the demonstra-tions.

where S = diag(s1, . . . , sd). Between the eigenvaluessi of the stiffness matrix and σi the following nonlinearinverse relationship holds:

si(σi) =

smin σi > σmax

p1 (1− tanh (p2)) + smin σmin ≤ σi ≤ σmax

smax σi < σmin

(10)

where

p1 =smax − smin

2, p2 =

2k0

smax

(σi −

σmax − σmin

2

). (11)

The stiffness values in each direction are bounded bythe tunable parameters smin and smax. The parametersσmin, σmin and k0 are also tunable parameters for thelearning system. The nonlinear relationship in Eq. (10),(11) is shown in Fig. 1. There are two saturation ar-eas corresponding to σi > σmax and σi < σmin wherethe eigenvalues assume the values smin and smax respec-tively. Among them there is an almost linear area inwhich si is inversely proportional to σi. The size of thesaturation areas and, consequently, the slope of the lin-ear part can be modulated by varying k0. To avoid rapidchanges in the stiffness values, the adopted function guar-antees a smooth transition between the linear area and thesaturation ones.

Fig. 1 Nonlinear relationship between covariance andstiffness matrices eigenvalues.

2.3 Control lawWe assume that our robot can be controlled by an

impedance control law (torque feedback) [9]. Given thedesired velocity, position and stiffness, the following con-trol law realizes the desired motion-impedance behavior:

τ = JTF + n(q, q, q) , (12)

where τ is the input torque, J is the Jacobian of the ma-nipulator and n(q, q, q) compensates the nonlinearitiesin the dynamical model of the robot.

The force term F is chosen as:

F =Kx+Dx , (13)

where K is the state-dependent stiffness matrix in Eq.(9), x is the velocity computed in Eq. (4) and x is ob-tained integrating x. The damping matrix D is cho-sen to have the same eigenvectors (principal directions)

of the stiffness matrix (Eq. (9)) with eigenvalues di =2√si, i = 1, . . . , d. Being the DS in Eq. (4) glob-

ally asymptotically stable andK,D positive definite, theforce term drives the robot towards the desired positionimposing a state dependent impedance behavior.

3. Simulation ResultsIn this section we validate the effectiveness of our ap-

proach in two cases. The first simulation is used to showhow the task constraints are learned from demonstrations.Synthetic 2-dimensional data are used. In the second ex-periment, a point-to-point task is learned from demon-strations.

3.1 Learning task constraintsIn this simulation we generate three 2-dimensional po-

sition trajectories2 that are constrained at the beginningand at the end of the motion. The trajectories start almostidentical, exhibit variations and end again identical.

The results obtained learning the DS in Eq. (4) withthree Gaussian components are shown in Fig. 2. As ex-pected, the algorithm is able to detect constraints in thedemonstrations and to return coherent values in terms ofposition and stiffness. The covariance matrices Σk

x, k =1, . . . , 3 are represented as ellipses in Fig. 2(a), where thedimension of each axis of the ellipse is proportional to thestandard deviation in that direction. The two covariancematrices close to the constrained areas have a small vari-ance, while the other has a big variance. As a results, thecovariance matrix Σx, computed by the regression tech-nique in Sec. 2.2, has a small standard deviation for pointsclose to the constrained areas, big otherwise (Fig. 2(b)).Conversely, the learned stiffness is big where the motionis constrained and small otherwise (Fig. 2(c)).

3.2 Point-to-point motionIn this simulation we learn a point-to-point motion

task from human demonstrations. The data, collected bykinesthetic teaching2, are shown in Fig. 3(a). The demon-strations have high variability at the beginning of the mo-tion, while they converge to the goal position at the end.

Again, the learning algorithm is able to capture thisvariability. The two learned covariance matrices Σk

x, k =1, 2 are represented as ellipsoids in Fig. 3(a), where thedimension of each axis of the ellipsoid is proportionalto the standard deviation in that direction. The covari-ance matrix closer to the initial points in the trajectorieshas bigger covariance than the other, being the variabil-ity at the beginning of the motion considerably bigger.The generated motion, obtained integrating the learnedDS velocity with a sample time δt = 0.01s, is shown inFig. 3(b). As expected, the trajectory converges to thegoal position, being the learned DS globally asymptot-ically stable. Figure 3(c) shows the eigenvalues of thecovariance matrix Σx. The eigenvalues have big valuesat the beginning of the motion (high variability) and de-crease while the trajectory converges to the goal position

2The velocity is computed by numerical differentiation.

−30 −20 −10 0 10 20 30−15

−10

−5

0

5

10

15

GMM

x2

x1

(a)

−30 −20 −10 0 10 20 30

−20

−15

−10

−5

0

5

10

15

20Regression

x2

x1

(b)

−30 −20 −10 0 10 20 30

−20

−15

−10

−5

0

5

10

15

20Stiffness

x2

x1

(c)

Fig. 2 Results of the learning algorithm on the synthetic dataset. (a) Demontrations (black lines) and learned model.(b) Smooth motion retrieved using GMR and related covariance matrix Σx. (c) Learned stiffness.

(a) (b)

0

20

40

60

80

50

100

150

0

5

10

0 50 100 150 200 250

Covariance matrix eigenvalues

λ1

λ2

λ3

iteration

00 50 100 150 200 250

0 50 100 150 200 250

x 103

(c)

0

0.5

1

0 50 100 150 200 250

Stiffness matrix eigenvalues

0

0.5

1

0

0.5

1

iteration

0 50 100 150 200 250

0 50 100 150 200 250

k1

k2

k3

(d)

Fig. 3 Results of the learning algorithm on the point-to-point motion dataset. (a) Demontrations (black lines) and learnedmodel. (b) Smooth motion retrieved using GMR. (c) Eigenvalues of the covariance matrix Σx for the generatedmotion. (d) Eigenvalues of the stiffness matrix, normalized to 1.

(low variability). Conversely, the learned stiffness ma-trix eigenvalues in Fig. 3(d) are small at the beginning ofthe motion (high variability) and increase while the tra-jectory converges to the goal position (low variability).The eigenvalues in Fig. 3(d) are obtained firstly scal-ing the standard deviation along each direction, i.e. thesquare root σi of the eigenvalues of Σx, in the interval[σmin = 1, σmax = 4] and then applying the inverserelationship in Eq. (10) with [smin = 0, smax = 1].

The learned DS and stiffness are then used to generatethe impedance behavior in Eq. (12)-(13). To this end,we used a dynamic simulator of a KUKA lightweight7 degree-of-freedom robot [17]. The manipulator end-effector is driven by the learned DS to reach the tar-get position g = [−0.6 0.18 0.25] m (the orientationis kept constant), starting from x(0) = [−0.56 −

0.42 0.22] m. The stiffness eigenvalues range is cho-sen as [smin = 50, smax = 300], while the sampletime is chosen as δt = 1ms. For comparison, the sameDS is used to drive the robot with a constant stiffnessK = diag(300, 300, 300).

Firstly, we compare the end-effector position error be-tween the executed trajectory and the generated (integrat-ing the DS) one. As expected, the robot is able to reachthe target both with constant and variable stiffness (seeFig. 4). With constant high stiffness, the robot stays sig-nificantly closer to the reference trajectory. Hence, as al-ready mentioned, if the goal is an accurate tracking anhigh stiffness is required.

Secondly, we test the proposed approach when a col-lision occurs. To simulate a collision, an impulsive ex-ternal force f = [20 0 0] N is applied for 5ms starting

0 2 4 6 8 10 12 14 16 18 20 220

0.002

0.004

0.006

0.008

0.01

0.012

0.014

0.016

time [s]

erro

r [m

]

End-effector position error

Constant stiffness

Variable stiffness

Fig. 4 Norm of the end-effector position error withconstant (blue dashed line) and variable (black solidline) stiffness.

0 0.5 1 1.5−0.568

−0.566

−0.564

−0.562

−0.56

−0.558

−0.556

−0.554

−30

−20

−10

0

10

20

0 0.5 1.5

End-effector position

End-effector acceleration

time [s]

x1 [

m]

a1 [

m/s

2]

referenceconstant stiffness

variable stiffness

Fig. 5 End-effector position and acceleration whenan impulsive force of 20N is applied along the x1direction. To clearly show the effects of the appliedforce, only the x1 axis (the most affected) and thefirst 1.5s of the trajectory are considered.

at t = 0.5s. As shown in Fig. 5, when the robot has asmall stiffness, the external force generates a big devia-tion from the reference trajectory. In this case, in fact, therobot accomplish the applied force. Instead, with highstiffness, the robot generates higher accelerations at theend-effector to suddenly react to the external disturbance.This results in a considerably smaller deviation, but into apossibly dangerous behavior. Hence, the learned behav-ior guarantees a compliant and safe interaction with theenvironment in a certain area of the state space (until acertain distance from the target). The high stiffness closeto the target point guarantees instead to reach and keepthe desired final position.

4. Conclusions

We presented an approach to simultaneously learnkinematic and dynamic aspects of a task from humandemonstrations. The probabilistic framework offered byGaussian mixture models was adopted to represent thetask in the compact form of an asymptotically stable dy-namical system. Hence, the convergence of the motiontrajectory to a desired goal position is always guaranteed.The covariance matrices of the mixture components, rep-resenting the variability along the demonstrations, wereused to retrieve a smooth estimation of a state dependent

stiffness matrix. The resulting stiffness is small (compli-ant behavior) where the demonstrations exhibit high vari-ability, big otherwise. A suitable impedance control lawis also presented to realize the desired impedance behav-ior.

The approach has been currently validated only in sim-ulation and on a simple point-to-point motion task. Ourfuture research will focus on considering more complextasks, as well as on implementing and testing the pro-posed solution on real robots.

Acknowledgement

This work has been partially supported by the Euro-pean Community within the FP7 ICT-287513 SAPHARIproject and Technical University of Munich, Institute forAdvanced Study, funded by the German Excellence Ini-tiative.

References[1] D. Lee and C. Ott, “Incremental Kinesthetic Teach-

ing of Motion Primitives Using the Motion Refine-ment Tube,” Autonomous Robots, Vol. 31, No. 2, pp.115-131, 2011.

[2] S. M. Khansari-Zadeh and A. Billard, “LearningStable Non-Linear Dynamical Systems with Gaus-sian Mixture Models,” Transaction on Robotics,Vol. 27, No. 5, pp. 943-957, 2011.

[3] A. Ijspeert, J. Nakanishi, P. Pastor, H. Hoffmannand S. Schaal, “Dynamical Movement Primitives:learning attractor models for motor behaviors,”Neural Computation, Vol. 25, No. 2, pp. 328-373,2013.

[4] S. M. Khansari-Zadeh and A. Billard, “A dynamicalsystem approach to realtime obstacle avoidance,”Autonomous Robots, Vol. 32, No. 4, pp. 433-454,2012.

[5] M. Saveriano and D. Lee, “Point Cloud based Dy-namical System Modulation for Reactive Avoidanceof Convex and Concave Obstacles,” Proc. of theIEEE/RSJ International Conference on IntelligentRobots and Systems, pp. 5380-5387, 2013.

[6] M. Saveriano and D. Lee, “Distance based Dynam-ical System Modulation for Reactive Avoidance ofMoving Obstacles,” Proc. of the IEEE InternationalConference on Robotics and Automation, 2014.

[7] S. Haddadin, S. Belder and A. Albu-Schaffer, “Dy-namic motion planning for robots in partially un-known environments,” IFAC World Congress, pp.6842-6850, 2011.

[8] H. Hoffmann, P. Pastor, D.-H. Park, and S.Schaal, “Biologically-inspired dynamical systemsfor movement generation: automatic real-time goaladaptation and obstacle avoidance,” Proc. of theIEEE International Conference on Robotics and Au-tomation, pp. 1534-1539, 2009.

[9] N. Hogan, “Impedance control: An approach to ma-nipulation: Part i, ii, iii,” Journal of Dynamic Sys-

tems, Measurement, and Control, Vol. 107, No. 1,pp. 1-24, 1985.

[10] H. Gomi and M. Kawato, “Equilibrium-point con-trol hypothesis examined by measured arm stiffnessduring multijoint movement,” Science Vol. 272, No.5258, pp. 117-120, 1996.

[11] E. Burdet, R. Osu, D. W. Franklin, T. E. Milner andM. Kawato, “The central nervous system stabilizesunstable dynamics by learning optimal impedance,”Nature Vol. 414, No. 6862, pp. 446-449, 2001.

[12] G. Ganesh, A. Albu-Schaffer, M. Haruno, M.Kawato and E. Burdet, “Biomimetic motor behav-ior for simultaneous adaptation of force, impedanceand trajectory in interaction tasks,” Proc. of theIEEE International Conference on Robotics and Au-tomation, pp. 2705-2711, 2010.

[13] S. Calinon, I. Sardellitti, and D. Caldwell,“Learning-based control strategy for safe human-robot interaction exploiting task and robot redun-dancies” Proc. of the IEEE/RSJ International Con-ference on Intelligent Robots and Systems pp. 249-254, 2010.

[14] L. Rozo, S. Calinon, D. G. Caldwell, P. JimenezC. and Torras, “Learning collaborative impedance-based robot behaviors” Proc. of the AAAI Confer-ence on Artificial Intelligence pp. 1422-1428, 2013.

[15] K. Kronander and A. Billard, “Online learning ofvarying stiffness through physical human-robot in-teraction,” Proc. of the IEEE International Confer-ence on Robotics and Automation, pp. 1842-1849,2012.

[16] D. A. Cohn, Z. Ghahramani and M. I. Jordan, “Ac-tive Learning with Statistical Models,” Journal ofArtificial Intelligence Research, Vol. 4, No. 1, pp.129-145, 1996.

[17] R. Bischoff, J. Kurth, G. Schreiber, R. Koeppe,A. Albu-Schaeffer, A. Beyer, O. Eiberger, S. Had-dadin, A. Stemmer, G. Grunwald and G. andHirzinger, “The KUKA-DLR Lightweight Robotarm - a new reference platform for robotics researchand manufacturing,” International Symposium onRobotics and German Conference on Robotics, pp.1-8, 2010.