A Control Scheme With a Novel DMP-Robot Coupling Achieving Compliance and Tracking Accuracy Under Unknown Task Dynamics and Model Uncertainties

A control scheme consisting of a novel coupling of a DMP based virtual reference with a low stiffness controlled robot is proposed. The overall system is proved to achieve superior tracking of a DMP encoded trajectory and accurate target reaching with respect to the conventional scheme under the presence of constant and periodic disturbances owing to unknown task dynamics and robot model uncertainties. It further preserves the desired compliance under contact forces that may arise in human interventions and collisions. Results in simulations and experiments validate the theoretical findings.


I. INTRODUCTION
I N RECENT years robots are increasingly used into more dynamic and uncertain environments and thus motion planning is being replaced by motion generation from dynamical systems which can encode rich kinematic behaviors learned by demonstrations.Dynamic movement primitives (DMPs) are the most popular dynamical systems introduced in [1] and revisited in [2].DMPs allow spatial and temporal scaling and are robust to perturbations from initial conditions and target changes.They further allow on-line coupling via sensory feedback from the controlled robot, such as phase stopping to stop the time evolution in case of disturbances [3].
In practice, both the robot's performance and its compliance to external contact forces are required for accurately tracking the desired trajectory and reaching of the desired target as well as ensuring human-robot collaborative behaviours and safety under collisions.It should be noted that accurate target reaching is important particularly in interaction tasks (e.g placing, pegin-the-hole, drilling) while ensuring safety under unexpected collisions is a major issue in collaborative robotics that usually operate in uncertain and dynamic environments.If the robot controller imposes high apparent stiffness for accurately following the desired trajectory generated by the DMP, it may compromise safety under unintentional contact and make collaboration harder.If, on the other hand, a low target stiffness is imposed for safety and collaboration, disturbance inputs arising from model uncertainties and unmodelled task and robot dynamics induce considerable errors which compromise accuracy in target reaching and trajectory following.Variable stiffness robot control combined with DMPs has been proposed to address these issues but to preserve the control system's passivity a tank energy approach is required [4], [5].To be effective tank energy levels should not be depleted and the stiffness update law should drop stiffness fast in case of external contact forces for fast compliant reactions.Tracking control under uncertainties has been addressed by various other control methods in the literature.Robust control methods achieve tracking performance to the expense of compliance to external forces [6].The same is true for optimal control approaches such as LQR [6] and MPC [7] if the cost related to tracking accuracy is set at high values.
Adaptive control [6] on the other hand is effective only for model parametric uncertainties.
Compliant movement primitives (CMP) have been proposed to improve the tracking accuracy while maintaining compliant robot behavior without requiring explicit models of task dynamics [8], [9].They require robots which allow active torque control and joint torque sensing.CMP consist of a pair of DMP and a torque primitive (TP); the former encodes task motion trajectories and the latter the corresponding torques that are obtained by executing the learned motion trajectories with a high gain feedback controller which ensures the required tracking accuracy under the assumption of a stable high gain reproduction of the demonstrated motion.Each motion trajectory encoded by the DMP is executed under different task conditions (velocity, load) producing different torque signals.Each TP is paired with the DMP resulting in a number of CMPs that are generalized via statistical learning since each possible task variation cannot be learned in advance [8].In the CMP approach, suitable torque signals are fed forward during the execution of a desired task variant combined with a low feedback gain controller; they improve accuracy while remaining compliant given the task variant lies within the training set.Instead of learning off-line the feed forward compensation term, DMP coupling terms from the robot's output are utilized during execution in applications that demand a desired actual or virtual contact force [10], [11].In [10], the recording of a (nominal) contact force during the grasping of an object is driving the activation of the coupling to adapt the learned kinematic behaviour to perception errors of target poses.In [11], the coupling term is learned after a few repetitions of the same task in the case of a bimanual task with independently controlled robots.
This work shares the objective of the CMP approach, i.e. that of tracking accuracy of a DMP encoded motion trajectory while being compliant to contact forces possibly arising in collisions.Its main result is a new control structure that is easily implementable.In contrast to previously published works [8], [9], [10], [11] it does not require the learning of a feedforward or coupling term (offline or iteratively) to compensate uncertain task dynamics.It is successfully employed by adapting to different uncertain dynamics (e.g. with the arm holding objects of different weight) without compromising compliance.It induces a variable apparent stiffness under interactions without the need of variable control gains.Its basic idea relies in a combination of a low feedback gain robot controller with a virtual reference trajectory that is produced by a variant of the learned DMP.Stability analysis under sinusoidal and constant disturbances using contraction theory is utilized to show accuracy in target reaching and following of the desired trajectory encoded in the learned DMP.Moreover, compliance for collaboration with humans and safety under collisions is maintained at the target level.Without loss of generality, the analysis in this work is confined to the robot's end-effector position.

A. Dynamic Movement Primitives
Dynamic movement primitives (DMP) have been used to encode rich kinematic behaviors that are recorded from a kinesthetic demonstration and can generalize to new goals [2].A DMP for encoding Cartesian position in point to point motion is given by a transformation and a canonical system: where y ∈ R 3 is the position, z ∈ R 3 is the scaled velocity of the movement, s ∈ R + is a phase variable introduced to avoid explicit time dependancy, τ ∈ R + is a temporal scaling factor, g ∈ R 3 is the desired target, y 0 ∈ R 3 is the initial position, f (s) ∈ R 3 is a non-linear forcing term and α z , β z , α s > 0 are scalar positive gains.By choosing α z = 4β z the linear part of the transformation system (1), (2) becomes critically damped for each position coordinate.The forcing term f (s) ∈ R 3 is defined as a linear combination of N Gaussian basis functions, which enable the encoding of any smooth demonstrated trajectory: where ψ i (s) = exp(−h i (s − c i ) 2 ), c i 's are the centers of Gaussian kernels distributed along the phase of the movement and h i 's the inverse widths while W i are the columns of the weight matrix W ∈ R 3×N calculated during learning by Locally Weighed Regression (LWR) to encode the motion pattern of the demonstrated trajectory.Notice that s converges exponetially to zero hence at the end of the motion the forcing term vanishes and consequently the system converges to the target y = g, z = 0.
The Bio-Inspired version of a DMP mitigates the disadvantages of the original version when the initial position and the goal coincide for a degree of freedom [12], and utilizes the following equation instead of (1): where k = α z β z and d = α z are scalar gains.

B. Impedance Control With Robot and Task Uncertainties
Consider the dynamic model of a 3-dof non-redundant manipulator in the 3-dimensional operational space with p ∈ R 3 , ṗ ∈ R 3 p ∈ R 3 , being the Cartesian position,velocity and acceleration of the robot tip respectively: (6) where Λ p (p) = [J(q)Λ −1 (q)J T (q)] −1 , C p (p, ṗ) = J −T (q)C (q, q) q − Λ p (p) J(q)q, F g (p) = J −T (q)G(q), u c = J −T τ c , F (t) ∈ R 3 a term arising from robot unmodelled dynamics like friction and backlash and unknown task dynamics like unknown loads, F ext are external contact forces, τ c the joint control input, Λ(q) ∈ R 3×3 the manipulator's inertia matrix, C(q, q) ∈ R 3×3 the centrifugal/Coriolis matrix, G(q) ∈ R 3 the gravity vector and J(q) the robot Jacobian; it is assumed that the robot moves away from singularities so that the Jacobian is invertible and defines a bijective mapping between the joint space and the operational space.
Let y d , ẏd , ÿd ∈ R 3 be a desired trajectory in the operational space that can be planned or provided by a DMP.Impedance control is a popular solution for trajectory tracking.It requires an estimate of the robot inverse dynamics together with a PD control scheme for the tracking error e t = p − y d as well as the feedforward of the desired acceleration [13].In the closed loop, it yields the following target linear dynamics set by the designer are the target inertia, damping and stiffness; F e (t) includes F (t) of unmodeled robot and task dynamics and the residual dynamics that are due to robot model parametric uncertainties.
In case F ext = 0, the unknown term F e (t) acts as a disturbance input to the system affecting tracking accuracy.To be robust to such disturbances a stiff robot is required (high feedback gains) that contradicts the desired compliance to external contact forces for safety and collaboration.

C. Contraction Analysis
Contraction analysis provides a simple and efficient way for analyzing the stability of non-linear dynamics [14], [15].The basic convergence principle of contracting systems states that if all neighboring trajectories converge to each other (contraction behavior) global exponential convergence to a single trajectory can then be concluded [14].More formally, Definition 1: [14] Given the system equations ẋ = f (x, t) a region of the state space is called a contraction region if the Jacobian ∂f ∂x is uniformly negative definite in that region.Theorem 1: [14] Given the system equations ẋ = f (x, t), 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.
Notice that, in a globally contracting autonomous system, all trajectories converge exponentially to a unique equilibrium point [14].The following proposition, which is utilized in our analysis is proposition 2 of [15] applied to autonomous systems.
Proposition 1: If ẋ1 = g 1 (x 1 ) is contracting and ẋ2 = g 2 (x 1 , x 2 ) is contracting for each fixed x 1 , then the following hierarchy is contracting.

III. THE PROPOSED CONTROL SCHEME
A block diagram of the proposed control scheme is shown in Fig. 1.Let the desired trajectory y d , ẏd , ÿd ∈ R 3 be generated by a bio-inspired DMP with phase stopping given by the following dynamic equations: where z d ∈ R 3 is the scaled desired velocity, y d0 ∈ R 3 is the initial position, e ∈ R 3 is a feedback error to be defined below, τ is the desired time duration of the movement, τ a ≥ τ is an adaptive time scaling parameter with a sig , c sig being positive parameters of a sigmoid function that regulate its slope and center.The phase stopping implemented in this way allows the abrupt increase of τ a after a certain error value.In fact, for high slope values and |e| < c sig τ a ≈ τ while for |e| > c sig τ a takes very high values stopping the DMP evolution.
Instead of providing the system with the desired trajectory we generate a reference trajectory y, ẏ, ÿ ∈ R 3 by a virtual DMP preserving the generalization properties of (8) with the following structure: where e = p − y ∈ R 3 is the tracking error from the virtual reference trajectory with p ∈ R 3 being the robot's actual position which may differ from the reference depending on the tracking accuracy achieved by the robot control system.Notice that the virtual DMP is similar to (8) with the addition of a coupling term at the acceleration level.This coupling term is a negative feedback of the virtual tracking error utilizing the same position control gain k with the linear DMP part.The latter is instrumental in the achievement of the control objective as it is shown in Section III-A.Moreover, unlike [10], [11] this coupling term does not utilize the deviation from the nominal behavior (y d ) but from the virtual reference y.
The virtual DMP is coupled with a robot control system which implements a target impedance Λ d , D d , K d ∈ R 3×3 introduced in Section II-B, between the input and the virtual tracking error: (10) where Λ d , D d , K d are defined for the rest of the paper as diagonal positive definite matrices and F e (t) includes unmodeled and residual dynamics and F ext are external contact forces discussed in Section II-B.Notice that the linear system (10) ensures perfect tracking only when the input is zero.Otherwise the virtual tracking error depends on the value of the target stiffness K d .The employment of the proposed scheme with low target stiffness gains, although it does not utilize e t = p − y d , achieves high tracking accuracy of y d with e t reaching practically zero at the target.As it is made clear in the following stability analysis, after a transient, the virtual trajectory tracking error e is of the same magnitude and opposite sign from the distance between the desired and virtual trajectory y − y d .Hence, the role of the virtual trajectory y(= y d − e) is to provide the robot with a reference which includes the desired trajectory displaced by the amount required to follow accurately y d .One can say that the virtual trajectory is in fact deceiving the robot control system so that it accurately follows the desired trajectory y d .
The most significant disturbances that arise from task dynamics and robot model uncertainties are mainly caused by gravitational forces affecting tracking accuracy.These are constant at stationary states.In the following stability analysis we consider the cases of constant and periodic disturbances in order to take into account disturbance inputs during motion.For a constant input, it is proved that p converges to y d .For a periodical input, it is shown that p converges within a small region around y d if the input frequencies are considerably lower than the bandwidth of the virtual DMP and the robot control system; these conditions depend on the values of k and K d .Moreover, the system is compliant under the presence of a bounded external contact force of limited duration, typically arising in unforeseen contacts.

A. Stability Analysis
We write the control system in state-space form with respect to the state: ) Notice that F e (t) + F ext can be regarded as the control system's input.Stability analysis proceeds as follows.First, we prove that in the absence of an input, i.e F e (t) + F ext = 0, the system described by ( 11) is globally contracting.For autonomous systems this means that all trajectories converge exponentially to the equilibrium ξ = 0.Then, we consider the cases of a constant and a periodic input with frequencies considerably lower than the bandwidths of the virtual DMP and the robot control system.For the first case we prove that the virtual tracking error e converges to a constant such that p = y d .Similarly, in the second case e converges to a trajectory such that p ≈ y d .
Setting F e (t) + F ext = 0, the system, consisting of the first three equations of ( 11) is an hierarchical combination of the linear system: and of ṡ = −α s /τ a .System ( 12) is globally exponentially stable, with a unique equilibrium at (e, ė) = (0, 0), since it is linear and Λ d , D d , K d are diagonal positive definite matrices and therefore, it is globally contracting.For (e, ė) = (0, 0) we have ṡ = −α s s/τ which is contracting.Therefore, using Proposition 1 we prove that the hierarchical combination of ( 12) and of ṡ = −α s /τ a is contracting.Finally, in order to prove that system (11) with zero input is contracting, we examine the system composed of the last two equations of (11) and prove that is also contracting.In particular, the second system: ) is contracting since for the fixed point (e, ė, s) = (0, 0, 0) we have τ a = τ , f (s) = f (0) = 0 and hence ( 13) is simplified to the following linear contracting system: Thus, using Proposition 1 we conclude that the hierarchical combination of the system composed of the first three equations of ( 11) and the last two equations of ( 11) is also globally contracting.Consequently, system (11) is contracting in the whole state space.Since, in a globally contracting autonomous system all trajectories converge exponentially to a unique equilibrium point [14], the equilibrium ξ = 0 is globally exponentially stable, which implies y = p = g.In order to proceed with the cases of a constant and periodic inputs, we reformulate the virtual DMP system with respect to the dynamics of the error e r = y − y d .To this aim we consider the forcing term f (s) as a sum of position, velocity and acceleration terms assuming perfect reconstruction of the demonstrated data: For tracking accuracy we are interested in disturbance inputs from unmodeled dynamics for which |e| < c sig hence τ a ≈ τ .Then substituting (15) in the virtual DMP differential equation: This system together with the robot control system dynamics (10) can be written in the standard second order linear system form as follows: where Then, the first equation of (17) implies that e converges to K −1 d F .Substituting e = K −1 d F in the second equation of (17), implies that e r converges to −K −1  d F which is of the same magnitude and opposite sign as compared to e. Notice that p − y d = e + e r .Hence p converges to y d .The speed of convergence of e r depends on the value of k while phase stopping allows a compliant reaction according to the value of K d .
Consider the periodic input Λ −1 d F e (t) = A sin ωt.Then, the first equation of ( 17) is a linear system driven by a periodic input; its steady state solution is: where φ = tan −1 ( Then, the steady state response is given by: The second equation of ( 17) is a linear system for e r driven by the periodic input of the virtual tracking error.Assuming that ω n2 ω, the steady state response of e r will be given by the following: which is minus the function e ss (t).We can therefore conclude that p converges to y d if ω n1 ω and ω n2 ω.Notice that these conditions depend on the values of k and K d .

IV. SIMULATIONS
We have simulated a 3-dof non-redudant manipulator in the 3-dimensional operational space driven by a DMP system, described by ( 8), ( 9), (10) 2 which consists of a constant and a sinusoidal component.We have also simulated the conventional scheme in which the system is driven by the DMP's desired trajectory (8) rather than the virtual trajectory for comparison purposes.Notice that ω n1 ω = 0.785 and φ = 89.4deg and that |e max | ≈ 0.019 which means that τ a = τ .Furthermore ω n2 ≈ 10 ω.The position response of the robot with the proposed scheme p 1 and the conventional scheme p 2 is shown in Fig. 3 together with the desired trajectory.Notice how the proposed scheme follows closely the desired trajectory reaching the target as opposed to the conventional scheme which exhibits significant errors in the z − axis where the disturbance input is higher.The norm of the tracking error is shown in Fig. 4 where practically a zero tracking error is achieved after a fast transient (see embedded subplot).To achieve the same magnitude of tracking errors with the conventional scheme we had to increase the robot control gain from 200 to 25000 and adjust the velocity gain accordingly, resulting to a non-compliant robot under collisions.To reveal the way the proposed scheme works, Fig. 5 shows the tracking error e t = p − y d together with the virtual tracking error e = p − y and e r = y − y d for each   axis.It is clear that the virtual tracking error e is compensated by e r so that the tracking error e t is zero after a fast transient.
Next, we repeated the simulation but now we have exerted a contact force shown in Fig. 6 of maximum value of 50 N that could have arisen in collisions.The response of the norm of the tracking error e t = p − y d is shown in Fig. 7 demonstrating the system's compliance in case of collisions.Moreover the response of p is shown in Fig. 8 for the z − axis where the contact force is higher together with the response of the respective virtual reference y with and without the external force.Notice how the evolution of y slows down during the external   force exertion delaying its compensating effect, thus allowing the compliant response of p.The apparent stiffness estimated as the ratio of maximum exerted force over the maximum error is approximately K a = 119 N/m which is practically close to the target stiffness (K d value).

V. EXPERIMENTAL RESULTS
For the experiments we have used a 7-dof KUKA LWR 4+ robot with a Barret BS8-hand attached at its wrist holding a cylindrical tin under gravity compensation.The robot was driven at the torque level with a control loop running at 500 Hz by a Jacobian transpose PD-control scheme of the Cartesian position error p − y with a low P-gain set at K p = 200I 3 for low positional stiffness and a velocity gain set at D d = 40I 3 .We have fixed orientation dofs to stay at their initial values by a high gain feedback controller.Notice that this compliant control scheme is equivalent to impedance control without inertia shaping and implies disturbance forces from uncompensated system dynamics unlike the simulation cases.The task is to place the tin on top of another on the table.The motion is demonstrated by human guidance and is encoded in a DMP with parameters d = 80, k = 1600, a sig = 500 and c sig = 0.05.The initial and target tip positions are p 0 = (−0.01m, 0.58 m, 0.18 m) and g = (0.34 m, 0.45 m, 0.41 m).In the first run, we demonstrate trajectory tracking and target reaching accuracy under unknown task dynamics by adding a weight of 0.5 kg on top of the grasped  tin, as unknown task dynamics.The experiment is repeated with the conventional scheme for comparison purposes.In the second run, we demonstrate the proposed scheme's collaborative aspects by having the human stop the robot's motion with its finger before reaching the target and place the weight on top of tin.In the last run, we demonstrate compliance under unintentional contact by having a human intersect the robot's path, which is now not carrying the weight.Disturbance inputs measured by the KUKA's external force estimator and the norm of tracking error for the first run are shown in Fig. 9 and 10 respectively.The larger disturbance values on z − axis are from the extra weight while disturbances in the x and y − axis are mainly due to the robot's uncompensated dynamics.Notice the low tracking error (<5 mm) at steady state and the accurate target reaching of the proposed scheme as opposed to the conventional scheme.In fact, the conventional scheme with errors in the range of 4.5 cm cannot complete the task colliding with the target tin (see the video).Results of the disturbance input and the actual and desired trajectory for the second run are shown in Figs.11 and 12. Notice the phase stopping effect as the user stops the robot at time 3.5 sec until he applies the weight at times 8 sec.After a short transient, the response is following the desired, reaching accurately the target (Fig. 12).In the last run, the contact force from the unintentional contact is depicted in Fig. 13, the robot's actual and desired trajectory in Fig. 14 and the norm of the tracking error in Fig. 15.Notice the phase stopping effect and the    compliant response of p as the user pushes the robot backwards mainly along the x − axis (see the video).The apparent stiffness can be estimated approximately at K a = 236 N/m which is almost equal to the target stiffness.The code used in the experiments can be retrieved from: https://github.com/kostasVlachos/dmp_control/releases/tag/DMP_control VI.CONCLUSION A novel control scheme is proposed to achieve both the following of a desired trajectory with accurate target reaching under robot and task uncertainties and compliance under intentional and unintentional contacts.It involves a virtual DMP reference trajectory coupled with a low stiffness controlled robot.The scheme is theoretically shown to achieve tracking error convergence in case of constant and sinusoidal disturbance inputs without requiring any prior training or iterative execution in order to perform well.Simulations validate the theoretical findings giving insights in the way the proposed scheme acts.Experimental results with a KUKA LWR4+ robot show accurate target reaching and higher tracking accuracy with respect to the conventional scheme.

Fig. 2 .
Fig. 2. Bounded disturbance input.Sum of a constant and a sinusoidal term.
with model and control parameter values set as follows: Λ d = I n , K d = 100I n , D d = 20I n implying ζ 1 = 1 and ω n1 = 10, k = 1600 d = 80 or a z = 80, b z = 20 for a critically damped response of the linear part of the DMP within approximately 0.4 s, a sig = 300 and c sig = 0.13 m.The DMP was trained with synthetic data from an initial position p 0 = (0 m, −0.2 m, −0.2 m) towards a goal g = (0.4 m, 0.2 m, 0.2 m) using LWR.The first simulation considers the case of an input F e (t) shown in Fig.

Fig. 3 .
Fig. 3. Position and desired trajectory under the bounded disturbance with the proposed (blue line) and the conventional scheme (black line).

Fig. 7 .
Fig. 7. Norm of the tracking error in the collision case.

Fig. 8 .
Fig. 8. Robot position and virtual DMP position reference in z − axis with and without the collision disturbance.

Fig. 14 .
Fig. 14.Position response for each axis under unintentional contact forces.