A novel DMP formulation for global and frame independent spatial scaling in the task space

In this work we study the DMP spatial scaling in the Cartesian space. The DMP framework is claimed to have the ability to generalize learnt trajectories to new initial and goal positions, maintaining the desired kinematic pattern. However we show that the existing formulations present problems in trajectory spatial scaling when used in the Cartesian space for a wide variety of tasks and examine their cause. We then propose a novel formulation alleviating these problems. Trajectory generalization analysis, is performed by deriving the trajectory tracking dynamics. The proposed formulation is compared with the existing ones through simulations and experiments on a KUKA LWR 4+ robot.


I. INTRODUCTION
In recent years robotic systems tend to leave their traditional confined and static workstations and are beginning to be integrated to unstructured human environments. In such environments, it is necessary for a robot to move in a more human-like manner, in order to be both safe for the humans around it and predictable. In that direction Imitation Learning and Programming by Demonstration (PbD) are gaining popularity [1]. An essential feature that any PbD framework must possess is the ability to generalize to new environments. Human environments are characterized by perpetual modifications and unpredictable alterations. Any learned movement must be able to be scaled both spatially and temporally, as well as be robust to perturbations.
Many PbD frameworks have been proposed in the literature. Such approaches involve optimization frameworks, probabilistic setups or dynamical systems. In [2] an optimization setup was proposed by minimizing simultaneously an accuracy and a jerk cost term. However as pointed out in [3] this framework fails to appropriately scale the trajectory spatially far from the demonstration workspace. Another optimization approach was proposed in [3], utilizing frame invariant descriptors [4], [5]. The approach is indeed successful in the desired trajectory generalization. However, as it is discussed in the same paper, this approach has large computation times for each control step.
Probabilistic approaches [6], [7], encode a large number of possible trajectory evolutions through multiple demonstrations. Then during execution the framework produces a trajectory according to some similarity measure with the demonstrated ones. These approaches are able to produce Authors  concrete results close to the demonstrated trajectory data, but fail to appropriately generalize the movement far away from the demonstrated trajectories.
Dynamical systems used for PbD are the DS [8] and DMP [9]. DS use a Gaussian Mixture Model setup to encode the trajectory characteristics. Their main disadvantages are the requirement for multiple demonstrations, and again the inability to generalize to new environment setup, outside the demonstration space. DMP offer an intuitive training process, as they require one demonstration to learn a desired trajectory, as well as spatial and temporal scaling characteristics. Due to their formulation as dynamical systems they achieve adaptations to uncertainties and perturbations, by being augmented with appropriate coupling terms [10], [11], or with adaptive equations for their parameters [12]- [14].
Despite their favorable properties, the spatial scaling of the DMP formulation in [9] is performed separately for each coordinate and hence it is frame dependant. This is undesirable in some tasks that demand the preservation of the 3D path pattern. In an attempt to alleviate this problem an alternative Biologically Inspired DMP formulation was proposed in [10], which however loses its global scaling property, as pointed out in [11] and [15].
In this paper we are going to showcase the problems arising from both the original and the Biologically Inspired DMP and propose a novel DMP formulation possessing global scaling abilities. This formulation is inspired by the observation made in [9], that DMP perform different motion generalizations by selecting different coordinate systems. We will demonstrate its effectiveness in various types of tasks encountered in robotic applications with simulations and experiments and compare it with the existing formulations.

II. DYNAMIC MOVEMENT PRIMITIVES
Dynamic Movement Primitives (DMP) are able to learn arbitrary trajectories in the Cartesian space by augmenting a single point attractor linear dynamical system by a non linear forcing term: where y, z ∈ R 3 are the position and the velocity scaled by τ ; g and y 0 are the goal and initial position, α z , β z are positive gains of the linear part of the system, chosen α z = 4β z for the system to be critically damped, τ is the temporal scaling parameter and diag (g − y 0 ) is a diagonal matrix with its elements being the coordinates of g − y 0 . Equation (1) is called the transformation system of the DMP.
The desired trajectory is encoded with the non linear function F (x), called forcing term: are N Gaussian kernel functions, with h i , c i their inverse widths and centers, and w i ∈ R 3 their weights to be learned in order to encode the desired trajectory. The weights are usually trained using Locally Weighted Regression. To avoid explicit time dependency the canonical system of the phase variable x is used: with its initial value x 0 = 1 and α x a positive gain. DMP where used for orientation trajectories using unit quaternions in [11] and extended in [16]. However in [17] we showed that those orientation formulations are problematic and proposed a different DMP orientation formulation: with e Q , z we denote the orientation error and its scaled by τ velocity, Q = η = exp − 1 2 e Q * Q g the orientation as a unit quaternion and Q 0 , Q g the initial and goal orientation.
The Biologically Inspired DMP framework proposed in [10], [18], [19] has the same structure with a modified transformation system: which can be generalized to the orientation space using the formulation of [17]: The original DMP framework (1), (4) scales the forcing term by multiplying it with the matrix diag (g − y 0 ). Conversely, the Biologically Inspired framework scales the forcing term by using the additive component −(g − y 0 )x.

III. DMP SPATIAL SCALING
To find out how both DMP formulations scale the trajectory, we will derive the trajectory tracking dynamics for each formulation. We will perform our analysis to the position DMP, however the same results apply to the orientation formulation. To train a DMP, the trajectory data Then the values of α z , β z and τ are chosen, with the most common choices for the temporal scaling parameter being τ = 1 and τ = T d . Utilizing the trajectory the desired forcing term is calculated. The resulting trained forcing term will be equal to the desired plus an error term t for each time step. Thus, for the original DMP: and for the Biologically Inspired: where indexes (t), (x) were omitted for better presentation. Substituting (7) to (1) we get the trajectory tracking dynamics of the original DMP: where matrix S g is defined as: and we also substituted g = S g (g d − y 0,d ) + y 0 .
From (9) it is clear that the trajectory is scaled according to the matrix S g . The term α z β z (y 0 − S g y 0,d ) serves the purpose of transferring the trajectory accordingly when beginning from a different initial position than the demonstrated. The training error is considered negligible, therefore we assume that the DMP generated trajectory follows the demonstrated one, scaled by S g and translated by α z β z (y 0 − S g y 0,d ). Notice that the scaling matrix is diagonal, which implies that scaling is performed separately for each coordinate and hence it is frame dependant. This behavior is in general problematic with a few exceptions. In fact, one cannot encode or execute trajectories when one coordinate has the same initial and goal value. This case often arises in tasks involving planar paths since then, the same coordinate value is involved along the axis normal to the plane. Moreover, when the initial and goal value are close to each other along one coordinate, the scaling is amplified and may demand positions and velocities that exceed the robot's limits or go through rigid working surfaces when e.g. a Cartesian task is executed over a workbench.
Regarding the trajectory tracking dynamics of the Biologically Inspired DMP we can substitute (8) to (5) to get : Notice that the term α z β z (g − g d ) has the effect of the appropriate trajectory displacement. From (11), it is also clear that the scaling is performed by the exponentially decaying term −α z β z x(g −y 0 −g d +y 0,d ) which affects the scaled trajectory additively. Thus problems of the original formulation are alleviated and for small initial position or goal displacements, the Biologically Inspired framework offers good results. However, when larger displacements are required, the learned trajectory pattern is ruined since the scaling term dominates the forcing term F (x). As the scaling term is dependant on the gains α z and β z , large values yield large initial trajectory displacements and velocities (where x = 1). Small gain values cannot remedy this problem as they deteriorate trajectory tracking (11).
We claim that the spatial scaling problems present in the current formulations stem from the frame coordinate dependence of the scaling. Our objective is a DMP formulation achieving global scaling, by preserving the motion pattern and its desired characteristics. We will consider free motion tasks in unconstrained spaces, planar tasks and tasks with orientation constraints. Planar tasks refer to demonstrated planar paths that should be generalizable to different planes. A lot of robotic tasks like spraying on tilted surfaces require such type of generalization. In such tasks scaling with the existing formulations, may generate non-planar paths. Furthermore the user cannot explicitly define the desired plane of task execution beforehand. When using the original DMP formulation in particular, many planar tasks cannot even be encoded. Tasks with orientation constraints refer to a great number of industrial (e.g. pick and place, assembly etc) and domestic (e.g. cutlery wielding, liquid pouring etc) tasks. Such tasks usually have targets that are constrained with regards to the orientation that is associated with the direction of the workbench normal vector. Such constraints cannot be handled by existing formulations and spatial scaling may result to trajectories that may even penetrate the workbench plane.

IV. PROPOSED DMP
To alleviate the original DMP framework's problems, and maintain its global scaling properties we propose the following DMP structure: where matrix R g is a rotation matrix that rotates a unit vector in the direction of g d − y 0,d to a unit vector in the direction of g − y 0 , and s g is a magnitude scaling constant, calculated as: The forcing term F (x) is given by (2) and the desired values of the forcing term are calculated from the demonstrated trajectory data as: The proposed formulation alleviates the problems of the original DMP as it does not scale each trajectory coordinate separately, but performs a three-dimensional scaling, involving both a rotation and a magnitude scaling. To study the spatial scaling of the proposed DMP we will again derive the trajectory tracking dynamics, by substituting F (x) = F d (t) + t to (12), using (14), assuming training error t : From (15) it is clear that the DMP generated trajectory is scaled by s g R g and translated by α z β z (y 0 − s g R g y 0,d ) to accommodate for different initial positions. Therefore, the trajectory will be accordingly rotated, by the matrix R g and then multiplied by the magnitude scaling constant s g . Due to the magnitude scaling being performed using the norms of g − y 0 and g d − y 0,d , the only case where the proposed formulation is not able to encode, is when the initial and the goal position are identical, instead of having just one equal coordinate as in the original DMP. However this property is generally not true for discrete motions.
For the orientation space, the proposed formulation can be written as: where similarly to the position R g is the rotation matrix that rotates a unit vector in the direction of 2 log (Q g,d * Q 0,d ) to a unit vector in the direction of 2 log (Q g * Q 0 ), and s g is a magnitude scaling constant, calculated as:

A. Selection of Rotation Matrix
The rotation matrix in (12) and (16) is a rotation matrix that rotates the demonstrated trajectory during the execution. For our analysis we denote n d = g d −y 0,d g d −y 0,d and n = g−y0 g−y0 . The same analysis yields for the orientation DMP of (16). Therefore R g is computed to rotate n d to n. The rotation matrix is not however unique by construct and its selection is highly dependant on the task to be executed. A rotation around any equivalent axis that lies on the plane normal to the plane defined by n d and n, passing from the line dissecting the angle between them when put in the same origin can be used. The dissecting plane's normal vector can be computed as: Thus, a set of candidate rotation axes are defined as follows: Concerning the selection of the rotation axis, we study the three groups of tasks discussed in Section III. For the free category of tasks, the trivial choice k = n d ×n suffices. This selection gives the minimal angle rotation between the two vectors, namely θ = cos −1 (n T d n). Then the rotation matrix can be computed by the Rodrigues' rotation formula [20]: In the case of the planar task, we consider the normal vector of the task plane during the demonstrationp d and during the executionp e . Then desired rotation matrix has to rotate both n d to n andp d top e . As n d andp d , as well as n andp e are orthogonal, the resulting rotation matrix is indeed unique and computed as: For the case of tasks executed over a workbench the rotated trajectory is expected to be least modified in the direction of the normal vector to the workbench plane, Fig. 1: Rotation axis for workbench tasks denotedp W . This requirement prevents the scaled trajectory from penetrating the workbench and violating orientation constraints. Thus, the rotation axis is chosen as the projection of the workbench plane normal vector to the dissecting plane: In Figure 1 an example of the rotation axis computation is depicted. The yellow plane is the workbench plane, and the gray the dissecting plane where S k lies. To calculate the rotation angle we use: Matrix R g is then computed from (20).

V. SIMULATION RESULTS
To demonstrate the proposed DMP formulation's spatial scaling capabilities we simulated (3), (12), (16) and compared the results with the original DMP formulation (3), (1), (4) and the Biologically Inspired DMP formulation (3), (5), (6). As desired trajectory data we used trajectories demonstrated by a user to a KUKA LWR 4+ robotic manipulator in gravity compensation. The desired trajectory begun from initial position and orientation y 0 In Figure 2 the proposed formulation is depicted, where the trajectory has been appropriately rotated and scaled,   maintaining the learnt pattern. In Figure 3 the original DMP formulation is shown. The trajectory is scaled using matrix (10) yielding S g,P = diag [1.19 0.23 2.88] T and S g,Q = diag [2.41 1.41 1.19] T . As the figure depicts, the scaling performed by the original formulation does not maintain the trajectory shape in three-dimensional space and leads to large scalings. Thus the robot's trajectory exceeds its workspace and penetrates the workbench plane. The Biologically Inspired DMP formulation is depicted in Figure 4. As explained in Section III this formulation generalizes the motion by an additive term. The effect of this term is clear at the beginning of the trajectory, where effect of the learnt pattern is minimal, and the generalization term is dominating. Thus the demonstrated motion profile is again not preserved. Figure 5 depicts the velocity and angular velocity norms of all formulations. The original DMP scales the velocity with the same matrix as the trajectory, leading to large velocity values. Due to the Biologically Inspired formulation's additive term, which is dependant of the phase variable there is a velocity peak in the beginning of motion, until the effect of the additive term diminishes. This peak however leads to non human-like behavior and can exceed the robot's velocity capabilities. The proposed formulation's velocity profile is higher than the demonstrated, in order to reach the new goal, but is kept to low enough levels to be able to be executed by a robotic system maintaining human-like characteristics.

VI. EXPERIMENTAL RESULTS
For our experiments we demonstrated trajectories to a 7 dof KUKA LWR 4+ robotic manipulator, in gravity compensation, with control frequency 500Hz. We then used the demonstrated data to train all DMP formulations and executed the systems with new initial and goal positions. During the execution the robot was under position control where the DMP generated velocity was fed to a Closed Loop Inverse Kinematics scheme [21], and mapped to the joint velocity with the Jacobian pseudoinverse. The resulting joint velocity was integrated to produce the commanded joint position.
We performed two experiments. In the first one, the robot was taught a planar task, and then was required to execute it on a different plane. In the second experiment a liquid pouring task was demonstrated, and then the robot was required to execute it with new initial and goal positions. A video of both tasks can be found in https://youtu.be/VgjZB3wVQ1c.
In both experiments all DMP formulations were trained using the data collected from the demonstrations. The parameters used for all DMP were α z = 40, β z = 10, N = 80, For the first experiment, the robot was taught to write the letter "R", on the plane z = 0 and then was tasked to execute the learnt motion pattern on the plane y   (21). The manipulator's orientation in both experiments is kept fixed, orthogonal to the task plane. The result of the first experiment is shown in Figure  6, where the trajectory of the robot, denoted p r is shown for the proposed and Biologically Inspired DMP formulations. The original DMP formulation fails to encode this task, as the initial and the goal position have the same Z coordinate in training and Y coordinate in execution. The proposed formulation is indeed successful in reproducing the pattern on the new plane. The Biologically Inspired formulation however fails to rotate the trajectory, and corrupts the pattern.  Figure 7 the proposed formulation has successfully generalized the trajectory, staying inside the robot's workspace and pouring the liquid inside the cup successfully. Conversely, in Figures  8 and 9 the trajectories produced by the original and the Biologically inspired DMP formulations are shown to fail to appropriately generalize the trajectory. In both cases the resulting motion would result in spilling the liquid. Furthermore, both trajectories are not able to be executed by the robot, as in the case of the original DMP the robot's workspace is violated and in the case of the Biologically Inspired DMP the robot's velocity limits are exceeded.

VII. CONCLUSIONS
In this paper we proposed a novel DMP for encoding a trajectory in the Cartesian space, which produces reasonably scaled trajectories globally, maintaining the learnt characteristics. We showed that spatial scaling of the original DMP formulation is problematic in a variety of tasks owing to the frame dependency of the trajectory generalization method while the subsequent Biologically Inspired formulation performs satisfactorily only locally. Our approach remedies this problem by appropriately rotating the executed trajectory and scaling its magnitude to alleviate frame dependency. The proposed formulation is shown to achieve the desired behavior through theoretical analysis, and validated through simulations and experiments.