Dynamic Movement Primitives for moving goals with temporal scaling adaptation

In this work, we propose an augmentation to the Dynamic Movement Primitives (DMP) framework which allows the system to generalize to moving goals without the use of any known or approximation model for estimating the goal’s motion. We aim to maintain the demonstrated velocity levels during the execution to the moving goal, generating motion profiles appropriate for human robot collaboration. The proposed method employs a modified version of a DMP, learned by a demonstration to a static goal, with adaptive temporal scaling in order to achieve reaching of the moving goal with the learned kinematic pattern. Only the current position and velocity of the goal are required. The goal’s reaching error and its derivative is proved to converge to zero via contraction analysis. The theoretical results are verified by simulations and experiments on a KUKA LWR4+ robot.


I. INTRODUCTION
In recent years, Dynamic Movement Primitives (DMP) have provided a way of encoding arbitrary complex motions utilizing non -linear dynamics [1]- [5]. This encoding provides accuracy in the execution of the motion, as well as the ability to generalize to spatial and temporal scaling. DMP have been used for a wide range of tasks. Both point to point or discrete motions, as well as rhythmic, periodic trajectories are able to be encoded, resulting in the encoding of various behaviours and motion patterns [6], [7]. Modified DMP have also been proposed to extend their robustness to temporal and or spatial perturbations [8], [9]. Moreover, appropriately designed coupling terms have been used for DMP adaptation to external inputs (e.g. force adaptation) [10]- [12]. DMP are proven advantageous over typical trajectory encoding mechanisms as they are able to generalize the learned pattern both spatially and temporally as well as be used in conjunction with other dynamical fields to generate in real time complex behaviours in environments with goal perturbations and obstacles [9].
Movements encoded by DMP are designed to be executed towards a stationary goal known before the start of the motion although it can be perturbed to another stationary goal during motion. Few research efforts have been focused on DMP utilization with a moving goal. In [13], the authors are modifying the DMP, to follow a moving goal in a leaderfollower framework. In [14] by augmenting a modified DMP formulation with velocity feedback the authors allow the system to follow moving goals. In both these works position Authors  and velocity trajectories are scaled towards the moving goal so that the preset time duration of the task is preserved. The latter may induce either higher or lower velocities depending on the current goal position. It is therefore possible that the generated velocity exceeds safety boundaries. The use of saturation on the robot's velocity does not resolve this problem as it generates a completely different velocity profile than that of the demonstration. However, in robot human coexistence, safety, trust and robot motion predictability is of outmost importance. It is therefore essential to maintain these human-like motion characteristics encoded during the demonstration.
A different solution to the problem of high velocities is to not require a fixed motion duration. This is possible in models that do not encode temporal characteristics of the motion [15]. Alternatively one could adapt the motion duration as in [16]; this adaptation is based on the utilization of a model for the goal dynamics which predicts the reaching location. However, such a model may not be available when for example the robot interacts with agents of unknown dynamic behaviour. Such agents could be other robots or humans whose motion patterns may not be necessarily known. In [17] a DMP formulation has been proposed in the context of hitting a moving ball with a specified velocity. This method has been extended in [18] utilizing the probabilistic DMP formulation proposed in [19]. Both formulations require again a predictor to estimate the reaching location and the moving goal's velocity at this point. Utilizing a predictor however causes the system to heavily depend on the prediction errors which can lead to execution failure.
The objective of this work is to enable a DMP, encoding a kinematic behaviour to a stationary target, to be utilized in the case of a moving target keeping velocities at levels similar to the demonstrated ones. Only the current goal's position and velocity is assumed to be provided by the perception system as opposed to the above works. To achieve this objective we propose a modified DMP for motion generation to a moving goal and prove its convergence to the goal using contraction analysis. Simulations and experiments are utilized to validate the theoretical findings. By alleviating the need of predictions and/or goal model dynamics the proposed approach does not depend on any prediction errors and addresses cases of unknown goal dynamics. Moreover, it addresses a wider range of DMP applications that involve collaboration with humans and work in shared environments in which maintaining a certain velocity level is important for the human safety and the robot's motion predictability. Such applications include collaborative tasks like handover or collaborative assembly in which the human (or another robot's) hand are considered as moving goals [13], [14].

II. PRELIMINARIES A. Dynamic Movement Primitives
Dynamic movement primitives (DMP) are used to encode trajectories, through the augmentation of a unique point attractor linear dynamical system with a non-linear term that encapsulates arbitrary complex shape modulations known as the transformation system of the DMP. For encoding an n degrees of freedom task, the transformation system is given by [5]: where y ∈ R n is the position, z ∈ R n is the scaled velocity, g ∈ R n is the attractor of the linear system, y 0 ∈ R n its initial position, diag (g − y 0 ) ∈ R n×n is a diagonal matrix with its elements equal to the coordinates of g − y 0 , τ is a scalar temporal scaling term, which traditionally is set equal to the duration of the motion and F (x) ∈ R n is the forcing term which determines the motion pattern: with w i ∈ R n being the weights of the N Gaussian kernel functions Ψ i : and c i the kernels' centers distributed to the interval [0, 1] and h i their inverse widths. Variable x is a monotonically decreasing phase variable, whose dynamics are known as the canonical system and are given by a first order system with its initial value set to 1: DMP are traditionally used for movements towards a constant goal g.
As mentioned earlier, the DMP framework is able to generalize to new goals. When a new stationary goal is selected, the demonstrated trajectory is scaled accordingly to the new goal's position. Therefore the generated position, velocity and acceleration will be the scaled demonstrated motion to the new goal. For example a goal which is twice as far from the initial position as compared to the demonstrated goal, will produce twice as large velocity and acceleration. As discussed in Section I this property may not be suitable for collaborative environments.
In case the new goal is not stationary but moving, the system may be eventually able to track the goal but the trajectory does not follow the encoded motion pattern after a preset time duration since the forcing term goes to zero when the phase variable vanishes. This is happening as the phase variable evolution (4) does not depend on g. For example when tracking a goal that moves away from the initial position, the phase variable will reach zero before reaching the goal, which implies a zero forcing term, thus removing all encoded motion patterns from the trajectory.

B. Contraction Analysis
Contraction Analysis was introduced in [20] and used in conjunction with DMP in [21] with the objective to provide a simpler method for the stability analysis of non linear dynamical systems. Instead of trying to verify point equilibrium stability, this framework tries to prove that if neighbouring state trajectories converge to each other, then all trajectories should exponentially converge to a single trajectory. This methodology can be expressed mathematically as follows: Definition I [20]: Given the system equationsẋ = f (t, x) a region of the state space is called a contraction region if the Jacobian ∂f ∂t is uniformly negative definite in that region. Theorem I [20]: Given the system equationsẋ = f (t, x), any trajectory, which starts in a ball of constant radius centered about a given trajectory and contained at all times in a contraction region, remains in that ball and converges exponentially to this trajectory. Furthermore, global exponential convergence to the given trajectory is guaranteed if the whole state space is a contraction region.
Consider a moving goal g(t),ġ(t). We assume that a perception system provides the goal's position and an estimate of its velocity on-line. The latter can be calculated by numerically differentiating the position measurements.
We propose the following DMP transformation system for the case of a moving goal: where e = g − y ∈ R n is the position error and z ∈ R n is the scaled velocity error. Notice that (5) is equivalent to (1) for the case of a stationary goal with the forcing term expressing the negative of that in (1). The system is trained as the traditional DMP framework, using a demonstration from the initial position y 0,d to the stationary goal g d with temporal scaling τ d .
In order for the system to generate a trajectory with the encoded motion pattern while reaching a moving goal, the temporal scaling parameter τ must be continuously adjusted. If the robot keeps the motion pattern of the demonstrated motion without adjusting its time scaling [13], [14] then, a faraway target would induce high velocities that could exceed the robot's capabilities. Hence, in the case of a moving target, the temporal scaling parameter should be modified in real time. We propose to augment the DMP system with the following temporal scaling adaption law: where κ τ is a positive design parameter and τ g is given by: Equation (6) ensures the tracking of τ g which can be interpreted as the temporal scaling related to the motion towards the current goal location g(t) considered stationary. When the goal moves away from the initial position y 0 the temporal scaling increases, leading to slower system evolution. This slow down counteracts the speed-up caused by the goal's position. When the goal approaches the initial position, τ decreases leading to faster execution. This again counteracts the slowing down caused by the goal's position.
In this way, the produced trajectory maintains the demonstrated velocity characteristics. Notice that τ g is bounded for g d = y 0,d which is generally true for discrete motions.
We also assume that the goal will not coincide with the initial position y 0 , g(t) − y 0 = 0, ∀t, which implies τ g > 0. The same assumption holds for (1) in order to enable motion encoding by ensuring a non-zero value for the term diag (g − y 0 )F (x). The derivativeτ g in (6) can be computed analytically from the goal's position and velocity. Notice that the proposed system does not require a model for the goal's motion, but only measurements of the goal's position and velocity. Further notice that after the moving goal is reached the forcing term vanishes; if then a disturbance temporarily displaces the arm away from the goal trajectory the DMP system may be re-initialized if a kinematic behavior similar to the demonstrated is desired. We claim that the system composed of (4), (5), and (6) with τ (0) = τ g (0) can achieve the objective of reproducing a desired kinematic behaviour while reaching a moving target.

A. Stability Analysis
The complete system can be written as: Stability analysis concerns the asymptotic convergence of the goal tracking error e = g−y and its velocity,ė =ġ−ẏ to zero. In addition, τ must follow the desired temporal scaling τ g , therefore the temporal scaling tracking error e τ = τ − τ g must also converge to zero.
We define the state vector s as follows: and write the system (8) in the state space form: The system consisting of the first two equations of (10) is a hierarchical combination of the linear first order system: and ofẋ = − 1 τ α x x. System (11) is globally exponentially stable, with a unique equilibrium at e τ = 0, as −κ τ < 0. Therefore the system is globally contracting. For e τ = 0, we have τ = τ g andẋ = − 1 τg α x x. As − 1 τg α x < 0, ∀t and the time depended parameter τ g is bounded,ẋ = − 1 τg α x x is also contracting. Therefore, using Proposition 1 we proved that the hierarchical combination of (11) and ofẋ = − 1 τ α x x is contracting. Finally, in order to prove that system (10) is contracting we will prove that the hierarchical combination of the system comprised of the last two equations of (10) is also contracting. In particular, the system: for the fixed point [e τ x] T = [0 0] T , which implies τ = τ g , F (x) = F (0) = 0, can be written as: which is a linear time varying system, with strictly negative eigenvalues for all values of the bounded time dependent parameter τ g . We denote I n the n × n identity matrix and 0 n the n × n zero matrix. Notice that τ g is bounded by the minimum and maximum distances between y 0 and g(t).
Theorem II [22]: Consider a linear non autonomous systeṁ x = A(t)x, x ∈ R n , A(t) ∈ R n×n . The equilibrium state x = 0 is exponentially stable if and only if for any given symmetric, positive definite, continuous and bounded matrix Q(t), there exists a symmetric, positive definite, continuously differentiable and bounded matrix P (t) such that: We select matrix P : P does not depend on time, therefore it is by construct bounded, and continuously differentiable, withṖ = 0. P is also positive definite (See Appendix). With this selection, we obtain: Q(t) is bounded by the maximum and minimum values of its eigenvalues: ∀x ∈ R 2n . Therefore Q(t) is bounded ∀t and using Theorem II the system of (13) is exponentially stable. Hence, the system is contracting. Thus using Proposition 1 we conclude that this hierarchy is also globally contracting. Consequently, system (10) is contracting in the whole state space. As a result, all trajectories converge exponentially to the unique equilibrium point s = 0, where e = 0,ė = 0 and e τ = 0 reaching the moving goal. The rate of this exponential convergence will be bounded between the rates corresponding to the largest, from above and smallest, from below value of τ .

IV. SIMULATIONS
For the demonstration with simulations of the system's performance the following setup was used. We have initially created a 3-D trajectory in the Cartesian Space to a stationary goal from y 0,d = [0.2 0.3 0.85] T to g d = [0.6 0.6 0.65] T with duration τ d = 1.124s. Then we trained the proposed DMP (5) utilizing these data. The DMP parameters used in the simulation are depicted in Table I. The value of α x was chosen so that x reaches 0.01 at the end of the demonstrated motion. To demonstrate the DMP capabilities in the case of a moving goal, we simulated the motion of a moving agent, from g 1 = [0.6 0.8 0.75] T to g 2 = [2 1.5 0.18] T . We then executed both the goal motion and the proposed DMP for moving goals (4) -6), from the initial position y 0 = y 0,d , feeding back the DMP with the current goal position and velocity. Notice that the system reaches the goal at the point g c = [1.92 1.44 0.33], at time t = 3.37s before the goal completes its execution and reaches g 2 . After reaching the goal the system tracks the goal's motion to g 2 . The simulation results are presented in Figures 1 -5. In particular, Figures 1, 2 and 3 show the path and trajectories of the y coordinates and its derivatives as well as those of the moving goal. We see the goal moving from a position g 1 near the stationary goal g d of the demonstration (red line path in Figure 1) following its own trajectory (black dashed line in Figures 2 and 3) towards g 2 which is further away from y 0 as compared to g 1 . The relative position and velocity errors converge to zero validating the theoretical analysis as shown in Figure 4. As shown in Figure 5, τ increases as the goal moves away from the initial position y 0 . The adaptation of τ prevents the appearance of high velocities as seen in Figure  3 keeping the velocity at levels similar to the demonstration. Notice in Figure 3, the discontinuity arising initially sincė g = 0 at t = 0. In practice, smoothness techniques should be employed at the beginning of motion. To demonstrate the advantages of the temporal adaptation law (6), Figure  6 shows the proposed DMP velocities together with those produced in a DMP execution without temporal adaptation (4), (5) with τ = τ d . Notice the higher velocities induced by the lack of temporal adaptation. This may not be acceptable as it may exceed the robot's capabilities, be dangerous for humans and ruin the human-likeness of the demonstrated motion.
Overall, simulation results show that the proposed DMP framework successfully reaches the moving goal, scaling the demonstrated motion's position pattern and subsequently tracks the goal motion, while maintaining velocity levels close to the demonstrated ones.

V. EXPERIMENTS
To demonstrate the proposed method on real world conditions, an implementation was made on a 7 degree of freedom KUKA LWR4+ robotic manipulator. The methodology was implemented in C++, on a real-time Linux PC. For the communication with robot, the FRI library [23] was used, with control frequency 500Hz. Robot kinesthetic guidance was used to record two trajectories. The recorded tool position data from the first demonstration was used to train the proposed DMP (4), (5) while data from the second demonstration was used to emulate a moving target. During both demonstrations the robot was under gravity compensation and high stiffness orientation control for maintaining its initial tool orientation.
The demonstrated trajectory started from initial position y 0,d = [0.39 0.32 0.24] T towards the goal position g d = [0.03 0.56 0.36] T in 6.54s. The moving goal's demonstrated trajectory started from the stationary goal of the demonstration g d , moving closer to the initial position and reaching g c = [0.19 0.44 0.31] T . During execution the DMP with the parameters of Table I generated the tool reference trajectory which was mapped to the joint space by the Jacobian pseudoinverse. Closed loop inverse kinematics [24] was utilized to ensure accurate following of the reference trajectory. The robot's initial position was set equal to the demonstrated one. When the moving goal was reached in 4.7s, the execution was terminated.
Results of the path, the position and velocity trajectory and the goal's trajectory are shown in Figures 7 -9. The robot's position and velocity coincide with the reference generated by the DMP. Position and velocity error convergence is shown in Figure 10. The temporal scaling parameter τ is shown to decrease in Figure 11 as the goal is approaching the initial position, from it initial value of τ d = 6.54 to τ = 3.63 at the end of motion. As the goal movement was simulated by a demonstrated motion, measurements of the velocity were available. However, we chose to numerically differentiate    the values of g, as it would happen if the goal's state was provided by a perception system. This differentiation explains the noise appearing in theġ andτ in Figures 9 and  11. The system however is not affected as it is evident from Figures 7 -11.

VI. CONCLUSIONS
In this paper a modified DMP framework is proposed for the case of reaching moving goals maintaining the demonstrated velocity characteristics. The framework only requires the goal's current position and velocity that can be in general provided by a perception system. Thus, it avoids prediction and model uncertainty errors that arise in predictor or model based approaches. Temporal adaptation provides predictable velocity behaviours that remain closer to the demonstrated as the moving goal scales the trajectory spatially. This generalization allows DMP to be used for more complex tasks and to operate effectively in workplaces shared with humans. The reaching of the moving goal is theoretically proved and validated by both simulations and experiments.

APPENDIX
Theorem III [25]: Consider a matrix that can be written as: with X ∈ R m×m , A ∈ R p×p , B ∈ R p×q and C ∈ R q×q , where m = p + q. The Schur complement of A in X is denoted X/A and defined as: If A and X/A are both positive definite, then X is also positive definite.