# Efficient DMP generalization to time-varying targets, external signals and
via-points

Antonis Sidiropoulos1 and Zoe Doulgeri1 1Aristotle University of Thessaloniki,
Department of Electrical and Computer Engineering, Thessaloniki 54124, Greece.
<EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

Dynamic Movement Primitives (DMP) have found remarkable applicability and
success in various robotic tasks, which can be mainly attributed to their
generalization and robustness properties. Nevertheless, their generalization
is based only on the trajectory endpoints (initial and target position).
Moreover, the spatial generalization of DMP is known to suffer from
shortcomings like over-scaling and mirroring of the motion. In this work we
propose a novel generalization scheme, based on optimizing online the DMP
weights so that the acceleration profile and hence the underlying training
trajectory pattern is preserved. This approach remedies the shortcomings of
the classical DMP scaling and additionally allows the DMP to generalize also
to intermediate points (via-points) and external signals (coupling terms),
while preserving the training trajectory pattern. Extensive comparative
simulations with the classical and other DMP variants are conducted, while
experimental results validate the applicability and efficacy of the proposed
method.

###### Index Terms:

Dynamic Movement Primitives, Programming by Demonstration

UNDER REVIEW

##  I Introduction

Dynamic Movement Primitives (DMP) has emerged as a promising method for
encoding a desired trajectory and generalizing it to new situations. It is
particularly favoured thanks to its ease of training, generalization and
robustness properties and has been applied in a plethora of practical robotic
tasks and applications. The trajectory to be encoded is typically provided
through PbD (Programming by Demonstration) [1], as an intuitive and efficient
means of teaching human-skills to robots.

In DMP, the objective of generalization is to generate a trajectory from a new
initial position to a new, possibly time-varying, target position, while also
preserving the shape of the demonstrated trajectory. To this end, different
types of generalization have been proposed with the most well-known being the
one introduced in [2]. This scaling has some important shortcomings, i.e.
over-scaling of the generalized trajectory when the demonstrated end-points
(initial and final position) are close, failure to generate a motion when the
new end-points during execution coincide and mirroring of the trajectory when
the sign of the difference between the new end-points is different from the
sign of the difference of demonstrated ones. Other DMP variants that attempt
to tackle the negative scaling effects include [3] and [4]. However, [3] can
still produce large spatial scalings and cannot generate a motion when the new
initial and final positions are the same, while [4] fails to reproduce the
demonstrated trajectory pattern to new targets that are relatively far away
from the demonstrated one.

Apart from the scaling issues of DMP, generalization to via-points and taking
into account within the generalization the effect of external signals has not
been addressed. Via-points could be specified for instance to locally adjust a
part of the DMP trajectory. Via-points with DMP were used in [5], but required
also specifying the velocity and acceleration of the via-point. This can
introduce significant noise, which is even more adverse if the via-points are
sparsely arranged, as shown in [6]. Via-points were also included, in an
offline optimization though, in a DMP-like variant in [7]. Other
methodologies, such as ProMP [8], are also capable of including via-points,
but are beyond the scope of DMPs that are studied here.

Coupling terms are usually employed in DMPs to adjust its trajectory to
external signals, e.g. forces arising from contact with the environment or
physical interaction with a human or ever virtual forces like the repelling
forces for avoiding limits or obstacles. Adaptation of a DMP trajectory to
external signals has been studied in various works [9, 10, 4, 11]. However, in
all these works, this external signal can significantly alter the shape of the
trajectory. Enabling adaptation to such external signals, while also
preserving the shape of the demonstrated trajectory is not addressed. A
different approach to incorporate external signals by learning an additional
coupling term through ILC (Iterative Learning Control) was investigated in
[12]. However, learning required multiple repetitions of the task and these
additional learned coupling terms are not accounted for in the DMP
generalization.

This work exploits the novel DMP formulation presented in our previous work
[13] to achieve online generalization of Dynamic Movement Primitives not only
w.r.t. new initial and/or target poses but also w.r.t. via-points and/or
external signals (coupling terms), while preserving the shape of the
demonstrated motion. At the same time, this novel generalization approach
remedies the shortcomings of the classical DMP spatial scaling. An additional
perk of the proposed method is that it ensures a smooth trajectory generation,
even if the provided target is discontinuous, guaranteeing that the target is
reached within a prespecified desired time duration. Finally, the proposed
novel generalization can also be combined with our previous work [14] to
impose kinematic inequality constraints. In [14], using the DMP formulation
from [13] with the classical DMP spatial scaling, an optimization framework
was designed that minimizes the distance from the unconstrained scaled
trajectory subject to robot kinematic inequality constraints, obstacle
constraints and via-points. The combination of this work with [14] improves
the generalization of the generated motion under kinematic constraints as
showcased through simulations.

The rest of this paper is organized as follows: In Section II we provide the
preliminaries for the novel DMP formulation from [13]. Section III is devoted
to the proposed novel online generalization method. Simulations that
demonstrate how the proposed method works and comparisons with the classical
DMP are provided in section IV. The practical usefulness and efficacy of the
proposed method is further showcased in the experimental Section V. Finally,
we draw the conclusions in Section VI. The source code for all conducted
simulations and experiments can be found at
github.com/Slifer64/novel_dmp_generalization.git.

##  II DMP formulation

Here we expound on the DMP formulation, which is based on the our previous DMP
formulation [13], but differs principally in the way that generalization is
achieved. In [13] we employed the same spatial generalization mechanism that
the original DMP [2] use. This generalization is known to be problematic in
some cases, resulting in large spatial scaling and in turn large velocities
and accelerations, it can cause undesired mirroring of the motion and fails to
produce a motion if the initial and target position coincide [4]. The proposed
formulation solves all these issues and it further allows us to achieve
generalization and adaptation to time-varying targets, external signals and/or
via-points, guaranteeing a smooth trajectory generation.

Consider the state $\bm{y}\in\mathbb{R}^{n}$, which can be joint positions,
Cartesian position or Cartesian orientation expressed using the quaternion
logarithm (see Appendix A for preliminaries on unit quaternions). The DMP
consists of two sets of differential equations, the transformation and the
canonical system. A DMP can encode a desired trajectory $t_{j},\ \bm{y}_{d,j}$
for $j=1...m$ where $m$ is the total number of points, and through its
transformation system it can generalize this trajectory spatially from an
initial position $\bm{y}_{0}$ to a desired target position $\bm{g}$. Temporal
generalization-scaling, i.e. execution of this trajectory with a different
time duration or speed, is achieved through the canonical system, which
provides the clock $s$ (time substiture) for the transformation system, to
avoid direct time dependency. Without loss of generality we consider a
canonical system where the phase $s$ evolves from $0$ at $t=0$ to $1$ at
$t=T_{f}$, where $T_{f}$ is the desired motion duration. The transformation
and canonical systems are given by [13]:

$\displaystyle\ddot{\bm{y}}$
$\displaystyle=\ddot{\bm{y}}_{s}-\bm{D}(\dot{\bm{y}}-\dot{\bm{y}}_{s})-\bm{K}(\bm{y}-\bm{y}_{s})$
(1) $\displaystyle\ddot{s}$ $\displaystyle=d(\dot{s}_{d}-\dot{s})\ ,\ s(0)=0\
,\ \dot{s}(0)=\dot{s}_{d}=\frac{1}{T_{f}}$ (2)

where $\bm{K},\ \bm{D}\in\bm{\mathcal{S}}_{++}^{n}$, with
$\bm{\mathcal{S}}_{++}^{n}$ denoting the set of symmetric $n\times n$ positive
definite matrices, and $\bm{y}_{s}$ encodes the desired trajectory through a
weighted sum of $K$ Gaussians given by:

$\displaystyle\bm{y}_{s}\triangleq\bm{W}^{T}\bm{\phi}(s)$ (3)
$\displaystyle\dot{\bm{y}}_{s}=\bm{W}^{T}\frac{\partial\bm{\phi}(s)}{\partial
s}\dot{s}$ (4)
$\displaystyle\ddot{\bm{y}}_{s}=\bm{W}^{T}\left(\frac{\partial^{2}\bm{\phi}(s)}{\partial^{2}s}\dot{s}^{2}+\frac{\partial\bm{\phi}(s)}{\partial
s}\ddot{s}\right)$ (5)

where $\bm{W}\in\mathbb{R}^{K\times n}$ are determined from
$\bm{W}=\text{argmin}_{\bm{W}}J(\bm{W}^{T}\bm{\phi}(s),\bm{y}_{d})$, where $J$
can be the Least Squares (LS), i.e.
$\text{min}_{\bm{W}}\sum_{j=1}^{m}||\bm{y}_{d,j}-\bm{W}^{T}\bm{\phi}_{j}||_{2}^{2}$),
or the Locally Weighted Regression (LWR) objective cost function [2]. The
Gaussian kernels are $\bm{\phi}(s)^{T}=[\psi_{1}(s)\ \cdots\
\psi_{N}(s)]/{\sum_{i=1}^{K}\psi_{i}(s)}$, with
$\psi_{i}(s)=exp(-h_{i}(s-c_{i})^{2})$ with $c_{i}$ being the centers and
$h_{i}$ the inverse widths. A reasonable heuristic is to set the centers
$c_{i}$ equally spaced in $[0,\ 1]$ and set
$h_{i}=1/(a_{h}(c_{i+1}-c_{i}))^{2}$ where $a_{h}$ controls the standard
deviation of the Gaussian kernels. Choosing $a_{h}\in[1.2,\ 1.5]$ produces
empirically good approximation results.

For encoding Cartesian orientation we can simply employ the quaternion
logarithm, i.e. $\bm{y}=\log(\bm{Q})$ where $\bm{Q}\in\mathbb{S}^{3}$ is the
orientation as unit quaternion. We can then obtain the quaternion, rotational
velocity and acceleration from $\bm{y}$ and its derivatives using the mappings
(15), (17) and (21) from Appendix A. Alternatively we can rewrite the
transformation system as:

$\dot{\bm{\omega}}=\dot{\bm{\omega}}_{s}-\bm{D}(\bm{\omega}-\bm{\omega}_{s})-\bm{K}\log(\bm{Q}*\bar{\bm{Q}}_{s})$
(6)

where $\bm{Q}_{s}$, $\bm{\omega}_{s}$ and $\dot{\bm{\omega}}_{s}$ are obtained
from (3)-(5) using (15), (17) and (21) from Appendix A.

###### Remark 1

Notice that in [13] we had defined
$\bm{y}_{s}\triangleq\bm{K}_{s}(\bm{f}_{p}(s)-\bm{f}_{p}(0))+\bm{y}_{0}$ with
$\bm{f}_{p}(s)=\bm{W}^{T}\bm{\phi}(s)$ and $\bm{K}_{s}$ being an appropriate
scaling matrix. To achieve mathematical equivalence with the original DMP [2],
i.e. to generate the exact same trajectories irrespective of the existence of
additional coupling terms, one has to choose
$\bm{K}_{s}=\text{diag}\left(\frac{\bm{g}-\bm{y}_{0}}{\bm{f}_{p}(1)-\bm{f}_{p}(0)}\right)$
[13]. Other scaling matrix choices are viable like the one proposed in [3]. As
we will show, this scalings can produce over-scaling of the demo trajectory
resulting in large velocities and accelerations. In contrast, here we adopt a
different approach for spatial generalization based on optimizing online the
DMP weights $\bm{W}$ so that the acceleration profile and hence the underlying
pattern of the training trajectory is preserved, without producing over-
scalings.

###### Remark 2

For Cartesian orientation in [13] we had defined
$\bm{y}=\log(\bm{Q}*\bar{\bm{Q}}_{0})$, where $\bm{Q}_{0}$ is the initial
orientation. This was necessary when using the spatial scaling matrix
$\bm{K}_{s}$, described in Remark 1, to ensure convergence to the target.
However, with the proposed formulation this is no longer necessary and we can
simply use directly $\bm{y}=\log(\bm{Q})$, as convergence to the target is
imposed as a constraint in the proposed optimization.

##  III Proposed generalization

Here we present a novel generalization scheme that allows a DMP to adapt not
only to new initial position $\bm{y}_{0}$ and target position $\bm{g}$ but
also to external signals and/or via-points111The term ”point” can refer either
to joint positions, Cartesian position, Cartesian orientation or Cartesian
pose., while preserving the shape of the demonstrated trajectory. To this end,
we propose to optimize the DMP weights so that the distance from the learned
(from the demonstration) acceleration profile is minimized, while also
satisfying the initial and final state constraints, i.e. start at $t=0$ from
$\bm{y}_{0}$ with zero velocity/acceleration and reach at $t=T_{f}$ the target
$\bm{g}$, again with zero velocity/acceleration. We assume that $\bm{g}$ can
be time varying between $0$ and $T_{f}$ and is constant afterwards. To achieve
this, the following optimization problem has to be solved at each time-step
$i$:

$\displaystyle\text{min}_{\bm{W}}$
$\displaystyle\sum_{j=1}^{m}\left|\left|{\frac{\partial^{2}\hat{\bm{y}}_{d,j}}{\partial^{2}s}-\bm{W}^{T}\frac{\partial^{2}\bm{\phi}_{j}}{\partial^{2}s}}\right|\right|_{2}^{2}$
(7a) s.t. $\displaystyle\bm{W}^{T}\bm{A}(0)=\bm{Y}_{0}$ (7b)
$\displaystyle\bm{W}^{T}\bm{A}(1)=\bm{G}_{i}$ (7c)
$\displaystyle\bm{W}^{T}\bm{\Phi}_{v,i}=\bm{Y}_{v,i}$ (7d)
$\displaystyle\bm{W}^{T}\bm{C}_{j}=\bm{Y}_{j}\ ,\ j=1...i$ (7e)

where the learned acceleration profile is
$\frac{\partial^{2}\hat{\bm{y}}_{d,j}}{\partial^{2}s}=\bm{W}_{0}^{T}\frac{\partial^{2}\bm{\phi}_{j}}{\partial^{2}s}$
with $\bm{W}_{0}=\text{argmin}_{\bm{W}}J(\bm{W}^{T}\bm{\phi}(s),\bm{y}_{d})$
(notice from (2) that, for nominal execution, $\ddot{s}=0$ and hence from (5)
the acceleration is just
$\frac{\partial^{2}\hat{\bm{y}}_{d,j}}{\partial^{2}s}$ scaled by the constant
$1/T_{f}^{2}$). The matrices in the first two equality constraints, (7b),
(7c), with $\bm{A}(s)=[\bm{\phi}(s)\ \dot{\bm{\phi}}(s)\
\ddot{\bm{\phi}}(s)]$, enforce the initial state $\bm{Y}_{0}=[\bm{y}_{0}\
\bm{0}_{n\times 1}\ \bm{0}_{n\times 1}]$ and final state
$\bm{G}_{i}=[\bm{g}_{i}\ \bm{0}_{n\times 1}\ \bm{0}_{n\times 1}]$ conditions.
Via-point constraints, which can be used to locally modify a segment of the
DMP trajectory, e.g. to constrain the motion in an insertion task or to push
aside or avoid obstacles, are defined in (7d), where
$\bm{Y}_{v,i}=[\bm{y}_{v,1}\ ...\ \bm{y}_{v,L_{i}}]$ contains the via-points,
with $L_{i}$ their number at the current time-step $i$, and
$\bm{\Phi}_{v,i}=[\bm{\phi}(s_{v,1})\ ...\ \bm{\phi}(s_{v,L_{i}})]$.222If the
phase $s_{v,l}$, $l=1...L_{i}$, is not provided explicitly, a reasonable
heuristic is to calculate it as
$s_{v}=\text{argmin}_{s_{k}}\left|\left|{\bm{y}_{s}(s_{k})-\bm{y}_{v}}\right|\right|$,
where $s_{k}$ in uniformly sampled in $(s,1]$. Empirically, taking
approximately $80$ samples is sufficient, as the minimization need not be
exact. The constraints in (7e) with
$\bm{C}_{j}=\bm{C}(s_{j})=[\bm{\phi}(s_{j})\ \dot{\bm{\phi}}(s_{j})\
\ddot{\bm{\phi}}(s_{j-1})]$ and $\bm{Y}_{j}=[\bm{y}_{j}\ \dot{\bm{y}}_{j}\
\ddot{\bm{y}}_{j-1}]$ encode the current and all previous state constraints up
to timestep $i$.

There are two different options for choosing $\bm{Y}_{j}$. The first one is to
set $\bm{Y}_{j}$ to the actual robot’s state at step $j$ so that any external
signals, included as coupling terms in (1), are accounted for (in way that the
preserves the demonstrated acceleration profile) in the DMP weights
adaptation. This can be desirable for instance, when human contact forces
intend to modify a segment of the learned DMP trajectory or when repulsive
forces are added as coupling terms to avoid obstacles. Having the DMP learn
online these adjustments allows the use of the final updated DMP weights in
future executions of the task ensuring the following of a desired path without
further adjustments. The second option is to set
$\bm{Y}_{j}=\bm{W}_{i-1}^{T}\bm{C}_{j}$, preserving the previously learned
trajectory, which would result in only local modulations of the output
trajectory in the presence of coupling terms. This is preferable when the
external signals are random non-recurring signals, e.g. unintentional contact
with a human or dynamic obstacle avoidance.

Regardless of the choice of $\bm{Y}_{j}$, the current state constraint for
$j=i$, ensures that a smooth trajectory is generated by (1) even in the
presence of abrupt target changes. The previous state constraints
$\bm{Y}_{1:i-1}$ guarantee that if the DMP is run in reverse as in [13], using
the final adapted weights from the forward execution, the same trajectory (in
reverse) will be executed. This could be expedient for safely retracting after
the forward execution of a task, especially if the task has geometric
constraints (like an insertion) which will have to be respected in the
retraction phase.

###  III-A Online DMP weights adaptation

Solving the problem in (7) on-line at each control cycle would be too slow and
hence prohibitive for real-time usage. Instead we can employ the following
recursive 2-step update at each time-step $i>0$, which has computational
complexity O($K^{2}$) and can be carried out in real-time:
step 1: Remove the effect of the previous target $\bm{g}_{i-1}$ and via-
points:

$\displaystyle\hat{\bm{W}}_{i}$
$\displaystyle=\bm{W}_{i-1}+\hat{\bm{K}}_{i}(\hat{\bm{Z}}_{i}-\bm{W}_{i-1}^{T}\hat{\bm{H}}_{i})^{T}$
(8a) $\displaystyle\hat{\bm{P}}_{i}$
$\displaystyle=\bm{P}_{i-1}-\hat{\bm{K}}_{i}\hat{\bm{H}}_{i}^{T}\bm{P}_{i-1}$
(8b) $\displaystyle\hat{\bm{K}}_{i}$
$\displaystyle=\bm{P}_{i-1}\hat{\bm{H}}_{i}(\hat{\bm{R}}_{i}+\hat{\bm{H}}_{i}^{T}\bm{P}_{i-1}\hat{\bm{H}}_{i})^{-1}$
(8c)

where

$\hat{\bm{Z}}_{i}=[\bm{G}_{i-1}\ \bm{Y}_{v,i}^{-}],\
\hat{\bm{H}}_{i}=[\bm{A}(1)\ \bm{\Phi}_{v,i}^{-}],\
\hat{\bm{R}}_{i}=-\epsilon\bm{I}_{3+b_{i}^{-}}$ (9)

with $\epsilon\approx 0^{+}$ and $\bm{Y}_{v,i}^{-}\in\mathbb{R}^{n\times
b_{i}^{-}}$ containing in columns the via-points that exist in the columns of
$\bm{Y}_{v,i-1}$ and not in $\bm{Y}_{v,i}$ (i.e. are present at time-step
$i-1$ and not at $i$)333Such is the case for instance if they are defined
relative to a varying target or an object, whose position has changed or has
been completely removed. and
$\bm{\Phi}_{v,i}^{-}=[\bm{\phi}(s_{k})]_{k=1:b_{i}^{-}}$ with $s_{k}$ being
the phase corresponding to each via-point in $\bm{Y}_{v,i}^{-}$.
step 2: Update to new target-constraints:

$\displaystyle\bm{W}_{i}$
$\displaystyle=\hat{\bm{W}}_{i}+\bm{K}_{i}(\bm{Z}_{i}-\hat{\bm{W}}_{i}^{T}\bm{H}_{i})^{T}$
(10a) $\displaystyle\bm{P}_{i}$
$\displaystyle=\hat{\bm{P}}_{i}-\bm{K}_{i}\bm{H}_{i}^{T}\hat{\bm{P}}_{i}$
(10b) $\displaystyle\bm{K}_{i}$
$\displaystyle=\hat{\bm{P}}_{i}\bm{H}_{i}(\bm{R}_{i}+\bm{H}_{i}^{T}\hat{\bm{P}}_{i}\bm{H}_{i})^{-1}$
(10c)

where

$\bm{Z}_{i}=[\bm{Y}_{i}\ \bm{G}_{i}\ \bm{Y}_{v,i}^{+}],\
\bm{H}_{i}=[\bm{C}_{i}\ \bm{A}(1)\ \bm{\Phi}_{v,i}^{+}],\ \bm{R}_{i}=\epsilon\
\bm{I}_{6+b_{i}^{+}}$ (11)

with $\bm{Y}_{v,i}^{+}\in\mathbb{R}^{n\times b_{i}^{+}}$ containing in columns
the via-points that are present at time-step $i$ and not at $i-1$ and
$\bm{\Phi}_{v,i}^{+}=[\bm{\phi}(s_{k})]_{k=1:b_{i}^{+}}$ with $s_{k}$ being
the phase corresponding to each via-point in $\bm{Y}_{v,i}^{+}$.

For initialization at step $i=0$ we set
$\bm{Z}_{0}=\begin{bmatrix}\bm{Y}_{0}&\bm{G}_{0}\end{bmatrix}$,
$\bm{H}_{0}=\begin{bmatrix}\bm{A}(0)&\bm{A}(1)\end{bmatrix}$,
$\bm{R}_{0}=\epsilon\bm{I}_{6}$, $\hat{\bm{W}}_{0}=\bm{W}_{0}$ and
$\hat{\bm{P}}_{0}=\bm{P}_{0}$ where

$\bm{W}_{0}=\scalebox{0.94}{\mbox{$\displaystyle\text{argmin}_{\bm{W}}J(\bm{W}^{T}\bm{\phi}(x),\bm{y}_{d})$}},\
\bm{P}_{0}=\scalebox{0.9}{\mbox{$\displaystyle\left(\sum_{j=1}^{m}\frac{\partial^{2}\bm{\phi}_{j}}{\partial^{2}s}\frac{\partial^{2}\bm{\phi}_{j}}{\partial^{2}s}^{T}\right)^{-1}$}}$

###### Remark 3

At the beginning of each execution, the DMP weights are initialized to the
values that have been calculated offline once during the DMP training from the
demonstration. Alternatively, if at a previous execution the DMP was adjusted
to some external signals, resulting to some final weights $\bm{W}_{f}$, and it
is desirable to retain this adjusted pattern at subsequent executions, one
could set $\bm{W}_{0}=\bm{W}_{f}$.

###### Remark 4

When $\bm{g}$ is constant and/or we don’t wish to adapt the DMP trajectory to
external signals or via-points, we can drop the constraints in (7d), (7e). It
suffices to carry the above update only once at the beginning with
$\bm{Z}_{0}$, $\bm{H}_{0}$ and $\bm{R}_{0}$ to obtain $\bm{W}_{1}$ and use
$\bm{W}_{i}=\bm{W}_{1}$ for $i>1$.

###### Remark 5

In practice, due to finite numerical precision, setting $\epsilon$ in
$\hat{\bm{R}}_{i}$, $\bm{R}_{i}$ very close to zero can make the matrix being
inverted in (8c) and (10c) ill-conditioned. We have found that in practice
choosing $\epsilon$ approximately in the range $(10^{-10},\ 10^{-6})$ does not
create any numerical issues and the constraints are satisfied within an error
tolerance of the order of $10^{-4}$ or even less depending on how small
$\epsilon$ is.

###### Remark 6

Despite using the same value $\epsilon$ in the above analysis for simplicity,
it is easy to verify that different values of $\epsilon$ can be chosen for
each type of constraint. The selected $\epsilon$ value can facilitate finding
a solution in the optimization problem (7) in case constraints cannot be
strictly satisfied (i.e. infinite accuracy). Then, the value of $\epsilon$
essentially relaxes equality constraints, by penalizing the cost function (see
Theorem 3 from Appendix B). Choosing different values of $\epsilon$ for each
type of constraint, allows to prioritize which of these constraints should be
satisfied with greater accuracy. Moreover, when adapting to external signals
which contain noise, higher values of $\epsilon$ can be used to suppress that
noise.

###  III-B Derivation/Proof of the weights adaptation

In the following, we prove that (8), (10) solve the initial optimization
problem given in (7). To this end, we will utilize the theorems provided in
Appendix B. Problem (7) can be written compactly as:

$\displaystyle\text{min}_{\bm{W}}$
$\displaystyle\text{tr}\\{(\ddot{\hat{\bm{Y}}}_{d}-\bm{W}^{T}\ddot{\bm{\Phi}})^{T}(\ddot{\hat{\bm{Y}}}_{d}-\bm{W}^{T}\ddot{\bm{\Phi}})\\}$
(12) s.t. $\displaystyle\bm{W}^{T}\bar{\bm{H}}_{i}=\bar{\bm{Z}}_{i}$

where
$\ddot{\hat{\bm{Y}}}_{d}=[\frac{\partial^{2}\hat{\bm{y}}_{d,1}}{\partial^{2}s}\
...\
\frac{\partial^{2}\hat{\bm{y}}_{d,m}}{\partial^{2}s}]\in\mathbb{R}^{n\times
m}$, $\ddot{\bm{\Phi}}=[\frac{\partial^{2}\bm{\phi}_{1}}{\partial^{2}s}\ ...\
\frac{\partial^{2}\bm{\phi}_{m}}{\partial^{2}s}]\in\mathbb{R}^{K\times m}$,
$\bar{\bm{Z}}_{i}=\begin{bmatrix}\bm{Y}_{0}&\bm{G}_{i}&\bm{Y}_{1:i}&\bm{Y}_{v,i}\end{bmatrix}\in\mathbb{R}^{n\times
m_{1}}$ and
$\bar{\bm{H}}_{i}=\begin{bmatrix}\bm{A}(0)&\bm{A}(1)&\bm{C}_{1:i}&\bm{\Phi}_{v,i}\end{bmatrix}\in\mathbb{R}^{K\times
m_{1}}$ with $m_{1}=6+3i+L_{i}$. Based on Theorem 4 problem (12) is equivalent
to:

$\displaystyle\text{min}_{\bm{W}}f_{i}(\bm{W})$
$\displaystyle\triangleq\text{tr}\\{(\ddot{\hat{\bm{Y}}}_{d}-\bm{W}^{T}\ddot{\bm{\Phi}})^{T}(\ddot{\hat{\bm{Y}}}_{d}-\bm{W}^{T}\ddot{\bm{\Phi}})\\}$
(13) $\displaystyle\
+\text{tr}\\{(\bar{\bm{Z}}_{i}-\bm{W}^{T}\bar{\bm{H}}_{i})^{T}\bar{\bm{R}}_{i}^{-1}(\bar{\bm{Z}}_{i}-\bm{W}^{T}\bar{\bm{H}}_{i})\\}$

for $\bar{\bm{R}}_{i}=\epsilon\ \bm{I}_{m_{1}},\ \epsilon\approx 0^{+}$. Given
now the solution at timestep $i-1$ for problem (13) we want to find the
solution for timestep $i$, for which the cost function can be written as:

$\displaystyle
f_{i}=\hat{f}_{i-1}+\text{tr}\\{(\bm{Z}_{i}-\bm{W}^{T}\bm{H}_{i})^{T}\bm{R}_{i}^{-1}(\bm{Z}_{i}-\bm{W}^{T}\bm{H}_{i})\\}$

where
$\hat{f}_{i-1}=f_{i-1}-\text{tr}\\{(\hat{\bm{Z}}_{i}-\bm{W}^{T}\hat{\bm{H}}_{i})^{T}\hat{\bm{R}}_{i}^{-1}(\hat{\bm{Z}}_{i}-\bm{W}^{T}\hat{\bm{H}}_{i})\\}$.
Invoking Theorem 5 the solution to $\hat{f}_{i-1}$ is given by (8). Using now
Theorem 3 the solution to $f_{i}$ is equivalent to the solution of
$\text{tr}\\{{(\bm{W}-\hat{\bm{W}}_{i})^{T}\hat{\bm{P}}_{i}(\bm{W}-\hat{\bm{W}}_{i})}\\}+\text{tr}\\{(\bm{Z}_{i}-\bm{W}^{T}\bm{H}_{i})^{T}\bm{R}_{i}^{-1}(\bm{Z}_{i}-\bm{W}^{T}\bm{H}_{i})\\}$,
which based on Theorem 2 is given by (10).

##  IV Simulations

In the following we compare the DMP produced with the proposed generalization,
henceforth referred as DMP++, with the classical DMP, as well as other
variations that have been proposed in the literature. We further demonstrate
and comment on the effects of adaptation to external signals, via-points and
also including kinematic limits (inequality constraints) using the proposed
generalization with [14]. Unless stated otherwise, in all cases, based on
Remark 6 and in the spirit that higher priority should be placed on the
initial and final position constraints followed by the via-point constraints
and then the previous state constraints, we chose $\epsilon$ $10^{-9}$ for the
initial and final position, $10^{-7}$ for the initial and final velocity and
acceleration constraints, $10^{-7}$ for via-points and $10^{-6}$, $10^{-6}$,
$10^{-4}$, for the previous state constraint. Notice that violation of the
previous state constraint generates a discontinuous acceleration in the DMP
(1), which will be larger the bigger the error in the equality constraint is.

###  IV-A Spatial generalization comparison

We start off by examining the three basic cases where the classical DMP
scaling is problematic [4]. For simplicity we consider an $1$ DoF
demonstration starting at position zero. Simulation results for each case are
plotted in Fig. 1, where the demo is plotted with green dashed line, the
proposed DMP++ with blue and the classical DMP with magenta dotted line. In
the first case (Fig. 1(a)) the demonstrated target $g_{d}$ is quite close to
the initial position $y_{0,d}=0$. In this case, even a new target that is
close to that of the demo, results in over-scaling for the classical DMP. In
the figure, the resulting trajectory for the classical DMP is scaled by $400$
for visualization purposes (the position actually converges to $g$ but in the
plot due to the scaling by $400$ it is $g/400\approx 0$). In the second case
(Fig. 1(b)) the demo initial and target positions are different but the new
target is the same with the initial position i.e. $g=y_{0}$. In this case, no
motion is generated by the classical DMP. Finally, in the third case (Fig.
1(c)), if the new target is chosen below the initial position, or more
generally when $\text{sign}(g-y_{0})=-\text{sign}(g_{d}-y_{0,d})$, the
classical DMP produces a mirrored trajectory. Unlike the classical DMP, the
proposed DMP++ retains the shape of the demonstrated trajectory in all cases,
without exhibiting any side-effects.

(a) The demonstrated start and goal positions are close.

(b) New goat set at the start position

(c) Mirroring.

Figure 1: Comparison with classical DMP generalization.

We further compare our method with the scaling proposed in [3], which we will
call DMP-rot, and the so called bio-inspired DMP variation from [4], referred
here as DMP-bio. Simulations were conducted, using as demo a Cartesian
position trajectory depicted with green dashed line in Fig. 2. Notice that the
demonstrated initial position $\bm{p}_{0}$ (green circle) and target position
$\bm{g}_{d}$ (light red cross) are not close. Despite that, the generated
trajectory to the new target $\bm{g}$ (vivid red cross) still produces a large
scaling for DMP-rot (cyan dash-dotted line), which is attributed to the fact
that all DoFs are scaled by $\left|\left|{\bm{g}-\bm{y}_{0}}\right|\right|$,
therefore even though the offset along the $z$-axis is moderate, the offset of
$\bm{g}$ by $1$ m in the $xy$-plane increases the overall scaling. DMP-bio
(light brown line) does not result in over-scaling, however the amplitude of
the trajectory is shifted, distorting slightly the demonstrated shape. This is
attributed to the fact that DMP-bio tracks a time-varying attractor that
shifts exponentially from tracking the demo w.r.t. the initial position to
tracking it w.r.t. the new target (this can be verified by analyzing the
forcing term in [4], analogous to the analysis in [13]). The result is a
drag/shift of the trajectory that gets more pronounced the further the new
target is from the demonstrated one. On the other hand, the proposed DMP++
(blue line) does not exhibit any of the aforementioned drawbacks.

Figure 2: Generalization comparison between the proposed and other DMP
variants.

###  IV-B Generalization to target perturbations

We further test out method in a scenario where the target position changes
abruptly at discrete time-steps and compare it to using the classical DMP with
goal-filtering [2], i.e. the DMP target is updated as
$\dot{\bm{g}}=a_{g}(\bm{g}_{\text{new}}-\bm{g})$. The results are plotted in
Fig. 3, where the target is shown with red. Both the proposed DMP++ (blue
line) and the DMP with goal filtering (magenta dashed line) produce continuous
trajectories and converge to the target. Nevertheless, the DMP with goal
filtering generates higher accelerations at the time-instants the target
changes, even though the filtering constant is selected at a value $a_{g}=4$
that yields a slow settling time of about $200$ ms.

Figure 3: Adaptation to perturbed target.

###  IV-C Adaptation to external signals

To demonstrate the ability of our method to adapt and generalize to external
signals and at the same time retain as much as possible the shape of the
demonstration, we consider an obstacle avoidance scenario, where the external
signals are the repulsive forces generated by a repulsive artificial potential
associated with each obstacle, activated along an offset $d_{0}$ from it as
defined in [15]. The method is presented in [15] for obstacle modeled by
spheres but it can be generalized for any other analytic smooth surface like
an ellipsoid or a plane which we use in this simulation. In particular,
denoting by $\mathcal{O}=\\{\bm{y}\in\mathbb{R}^{n}:\psi_{o}(\bm{y})=0\\}$ the
surface of an obstacle, for an ellipsoid we have
$\psi_{o}(\bm{y})=(\bm{y}-\bm{c})^{T}\bm{\Sigma}^{-1}(\bm{y}-\bm{c})-1$, where
$\bm{c}$ is the center and $\bm{\Sigma}$ the covariance, and for a plane
$\psi_{o}(\bm{y})=\bm{n}^{T}(\bm{y}-\bm{y}_{0})$, where $\bm{n}$ is the vector
normal to the plane and $\bm{y}_{0}$ any point on the surface plane. By
defining the normalized distance
$e=\frac{(\psi_{o}(\bm{y})-d_{0})^{2}}{d_{0}^{2}}$ if $\psi_{o}(\bm{y})\leq
d_{0}$, otherwise $e=0$ and the barrier potential $V_{o}=-\log(1-e)$ which is
zero for $\psi_{o}(\bm{y})\geq d_{0}$ and tends to $+\infty$ on the surface
i.e. as $\psi_{o}(\bm{y})\rightarrow 0$, a valid repulsive force that prevents
the penetration of the surface can be obtained as
$\bm{f}_{\text{rep}}=-k_{o}\frac{\partial V_{o}}{\partial\bm{y}}$, where
$k_{o}>0$ controls the magnitude of the repulsive force. Adding
$\bm{f}_{\text{rep}}$ for each obstacle as a coupling term in (1) achieves
obstacles’ avoidance.

In the simulation, a demonstration of a path in an obstacle-free space shown
with green dotted line in Fig. 4, is executed in the presence of 2 ellipsoid
and 2 plane obstacles shown with dark grey lines444The boundary determined by
$d_{0}$ around each obstacle is shown with light grey. for both the DMP and
the proposed DMP++ with repulsive forces added as coupling terms. Given this
static scene, we execute the task both in forward and reverse directions. For
the reverse (rev-DMP++) we set $\bm{W}_{0}=\bm{W}_{f}$, where $\bm{W}_{f}$ are
the adapted weights from the forward execution, according to Remark 3. To
enable the DMP++ to adapt to external signals we set the current state
constraint to the current actual state as explained in Section III. Moreover,
reflecting on Remark 6, we choose smaller values for $\epsilon$ ($10^{-4}$,
$10^{-1}$, $10^{-1}$ for position, velocity and acceleration), treating
repulsive forces as noisy signal, since, despite being continuous, they can
generate abrupt signals that significantly affect the acceleration and in turn
the position and velocity.

The resulting paths for forward execution of DMP++ (DMP) is shown with blue
(red) line in Fig. 4 followed by the reverse execution shown with cyan
(mustard) line. The corresponding repulsive forces are shown on the right
subplot of Fig. 4. Both DMP and DMP++ avoid the obstacles thanks to the
repulsive forces. However, the shape of the path executed by the DMP both in
forward (red line) and in reverse directions (mustard line) saturates on the
obstacle boundaries, distorting the demonstrated shape (green dotted line).
Moreover, as the DMP does not adapt its weights to the repulsive forces, much
higher forces are generated both in forward and reverse execution (Fig. 4
right subplot). In contrast, DMP++ adapts its weights to the repulsive forces
in a way such that the demonstrated acceleration profile is preserved. This
results in lower repulsive forces, which due to the adaptation become even
smaller during the reverse execution and also a path shape similar to that of
the demonstration is retained. Notice that if we disable the adaptation to
external signals, by setting the current and previous state constraints
$\bm{Y}_{j}=\bm{W}_{i-1}^{T}\bm{C}_{j}$ as explained in Section III, then
DMP++ would produce the exact same trajectory as the DMP.

Figure 4: DMP vs DMP++ with repulsive forces from obstacles. Left: Cartesian
paths and obstacles with dark grey along with the boundary for activation of
the repulsive force, determined by $d_{0}$, with light grey. Right: Generated
repulsive forces during forward and reverse execution.

###  IV-D Adding kinematic inequality constraints

Figure 5: Combination of the proposed generalization with [14] to further
impose kinematic inequality constraints. The image is taken from [14], where
the differences/additions are highlighted with magenta. DMP++ replaces the DMP
with the classical scaling. Via-points and external signals are also
considered during the generation of the generalized (unconstrained) trajectory
$\bm{y}_{d}$.

Here we compare with the framework in [14] which uses the DMP formulation [13]
with the classical DMP scaling to generate the generalized trajectory
$\\{\bm{y}_{d},\dot{\bm{y}}_{d},\ddot{\bm{y}}_{d}\\}_{i}$ over a horizon of
$N$ future time-steps which is then optimized to respect kinematic limits like
position velocity and acceleration bounds as well as passing from via-points.
We show that by modifying the framework from [14] according to Fig. 5, where
the DMP with the classical scaling is replaced by DMP++ we achieve more
efficient generalization that accounts also for via-points and external
signals. Notice that via-points are included both in DMP++ and in the
optimization module of [14] (Fig. 5 brown rectangle) via the respective
equality constraints so as to guarantee that the constrained within the
kinematic inequality limits trajectory will also pass from the via-points.

Simulation results of a $1$ DoF example with two via points and position,
velocity and acceleration limits are shown in Fig. 6, where the method
proposed in [14] is used to optimize the velocity profile with all relevant
parameters chosen as in [14]. To endow the optimizer with greater flexibility
in finding feasible solutions we consider the kinematic limits as hard limits
(grey dashed lines) and introduce the lower and upper soft limits (magenta
dashed lines) within the hard limits. We want to preferably operate within the
soft limits and only exceed them if feasibility would be inevitable otherwise
[14]. It can be observed that the DMP++ (blue line) retains the shape of the
demonstration (green dash dotted line), even in the presence of kinematic
limits and via-points (red asterisks). On the other hand, the DMP with the
classical scaling (mustard dotted line) can induce large scalings that
generate velocities and accelerations that violate considerably the limits,
leading to saturation (see the $2$nd and $3$rd subplots of Fig. 6) and
consequently the distortion of the demonstrated shape, as can be observed
comparing the mustard with the green trajectory in the first subplot. This
distortion would be even more adverse for cases in which scaling is
problematic like those presented in Fig. 1, where a feasible solution may even
not be found.

Figure 6: Comparison of the classical DMP generalization against the proposed
DMP++ in combination with [14] to enforce kinematic inequality constraints.

##  V Experimental Results

To validate experimentally our method we consider a scenario where a robot has
to pick a carton from a conveyor belt and insert it inside a box. The pose of
the carton and the box are tracked using apriltags with a frame rate of $30$
Hz. Two DMP are trained through PbD, one for picking and the second for
inserting the carton in the box. We examined the two scenarios depicted in
Fig. 7. In the first (top picture), the carton is moving on the conveyor belt
and a perturbation of its pose is also introduced by having a human displace
it during execution. In the second scenario (bottom picture), an obstacle
obstructs the robot from picking the carton. In this scenario two variants are
examined: a) introducing a coupling term from a force exerted by a human, who
intervenes to guide the robot over the obstacle or b) using via-points,
obtained from an apriltag that the human uses to demonstrate a detour around
the obstacle.

In all experiments, to guarantee that the carton is properly inserted in the
box, we specified $5$ additional via-points with the same orientation with the
carton’s target with $3$ cm offset between them starting from the target
position. Moreover, to further highlight the robustness of our method, in all
experiments during the placing of the carton, the target pose of the box (and
hence its associated via-points) are altered on the fly.

Figure 7: Experimental setup.

To carry out these experiments a velocity controlled ur5e robot is used, with
$2$ ms control cycle, which takes as reference the velocity produced by the
DMP transformation system from (1), where $\bm{y}$ is either the Cartesian
position or for orientation $\bm{y}=\log(\bm{Q}*\bar{\bm{Q}}_{0})$. We also
add in (1) the coupling term
$\bm{u}=\bm{J}_{i}\bm{M}_{i}^{-1}\bm{f}_{i}-a_{i}\ddot{\bm{y}}_{s}$, where
$i\in\\{p,o\\}$ for position and orientation respectively,
$\bm{J}_{p}=\bm{I}_{3}$, $\bm{J}_{o}=\bm{J}_{\eta}^{T}$ (see (25)),
$\bm{M}_{p}=5$ and $\bm{M}_{o}=2$ can be interpreted as the desired inertia in
(1), with damping $\bm{M}_{i}\bm{D}$ and stiffness $\bm{M}_{i}\bm{K}$,
$\bm{f}_{i}$ is the external force/torque measured by the robot’s F/T sensor
at its wrist and $a_{i}\ddot{\bm{y}}_{s}$, with
$a_{i}=\sqrt{\text{min}(2\left|\left|{f_{i}}\right|\right|,1.0)}$, is used to
mitigate the effect of the feed-forward acceleration $\ddot{\bm{y}}_{s}$ of
the DMP whenever the external force/torque is greater than $0.5$ N/Nm. This is
useful when there is undesired contact or when the human guides physically the
robot to alter its trajectory. The DMP weights are updated online according to
(8), (10), while $\epsilon$ was set as in the simulations. The current state
constraint in the optimization is set to the actual robot’s state. In all
cases, the optimization at each control cycle was below $0.8$ ms, which is
well within the $2$ms control cycle of the robot. For the canonical system
from (2), we define
$\dot{s}_{d}=\dot{s}_{1}/(1+a_{d}\left|\left|{\bm{f}_{p}}\right|\right|)$,
$a_{d}>0$, which is used to enable phase stopping when an external force is
applied. For forward execution we set $s(0)=0$ and
$\dot{s}(0)=\dot{s}_{1}=\frac{1}{T_{f}}$, while for reverse (retraction)
$s(0)=1$ and $\dot{s}(0)=\dot{s}_{1}=-\frac{1}{T_{f}}$. Also, for the case
that we want the DMP to adapt to external signals (experiment 2 - case (b)) we
disable phase stopping, i.e. $\dot{s}_{d}=\dot{s}_{1}$, as in this case the
measured external force is used to adapt the DMP trajectory and is not meant
to stop its evolution.

A video with the complete experimental scenarios along with extensive
explanations and visualizations can be found at
dropbox.com/s/9vfgs2qc8bwsbbt/video.mp4. Indicative results from scenario 1
(Fig. 7, top picture) are plotted in Fig. 8. On the top subplot, it can be
observed that continuous changes or even abrupt changes of the target during
pick (red line) and place (magenta) are handled smoothly by the proposed DMP
(blue and cyan line). On the bottom subplot, the phase variable evolution is
plotted, where during placing at $t\approx 12$ sec, the human holds the
robot’s end-effector and the exerted force (mustard line) triggers phase
stopping, while in the meantime the human displaces the box (top plot, magenta
line).

For the second scenario, the results in Fig. 9 demonstrate on the top the
adjusted DMP trajectory during the pick (brown line) along the $x$-axis, where
the maximal deviation from the nominal trajectory (magenta dashed line)
occurred. This adaptation required a moderate human force (mustard line) of
less than $6$ N. On the bottom subplot, the paths for the pick and place are
drawn, where for the place the associated via-points are also visualized, from
the initial to the finally displaced position of the box. Finally, the same
scenario is revisited, only this time the human does not interact physically
with the robot. Instead, he uses an apriltag as indicator of the via-points’
pose, through which the robot should pass. Results are presented in Fig. 10.
where on top, the via-points and the DMP adjusted trajectory along the
$x$-axis is plotted, and on the bottom, the $3$D paths and the via-points are
visualized.

Figure 8: Experiment $1$. Top: $y$ position of the DMP (blue line) for picking
the carton (red line) and the DMP (cyan line) for placing the carton in the
box (magenta line). Bottom: Phase variable evolution for picking (blue) and
for placing (cyan) superimposed with the external force (mustard line). Figure
9: Experiment $2$-case a: Adaptation of DMP to external force. Top:
$x$-position of nominal (dashed magenta line) and the modified DMP (brown
line) for picking the carton, superimposed with the applied force (mustard
line). Bottom: Cartesian path of the DMP for picking and placing the carton in
the box. Figure 10: Experiment $2$-case b: Adaptation of DMP with via-points.
Top: $x$-position of nominal (dashed magenta line) and the modified DMP (brown
line) for picking the carton. Bottom: Cartesian path of the DMP for picking
and placing the carton in the box.

##  VI Conclusions

In this work we presented a novel generalization scheme for DMP, based on
optimizing online the DMP weights, so as to preserve the demonstrated
trajectory shape. The proposed generalization has been shown to achieve
efficient generalization even in cases where the classical DMP generalization
was problematic. Moreover, we demonstrated how this method can extend the
online adaptation and generalization properties of DMP, through the
incorporation of via-points and external signals. All the aforementioned
properties were extensively validated through simulations and experiments.

## Appendix A - Unit Quaternion Preliminaries

Given a rotation matrix $\bm{R}\in SO(3)$, an orientation can be expressed in
terms of the unit quaternion $\bm{Q}\in\mathbb{S}^{3}$ as $\bm{Q}=[w\
\bm{v}^{T}]^{T}=[\cos(\theta_{2})\ \sin(\theta_{2})\bm{k}^{T}]^{T}$, where
$\bm{k}\in\mathbb{S}^{2}$, $\theta_{2}=\theta/2$ with $\theta\in[-\pi,\pi)$
are the equivalent unit axis - angle representation. The quaternion product
between the unit quaternions $\bm{Q}_{1}$, $\bm{Q}_{2}$ is denoted as
$\bm{Q}_{1}*\bm{Q}_{2}$. The inverse of a unit quaternion is equal to its
conjugate which is $\bm{Q}^{-1}=\bar{\bm{Q}}=[w\ -\bm{v}^{T}]^{T}$. The
logarithmic $\bm{\eta}=\log(\bm{Q})$ and exponential $\bm{Q}=\exp(\bm{\eta})$
mappings $\log:\ \mathbb{S}^{3}\rightarrow\mathbb{R}^{3}$, $\exp:\
\mathbb{R}^{3}\rightarrow\mathbb{S}^{3}$ respect the manifold’s geometry and
are defined as follows:

$\log(\bm{Q})\triangleq\left\\{\begin{matrix}2\cos^{-1}(w)\frac{\bm{v}}{||\bm{v}||},\
|w|\neq 1\\\ [0,0,0]^{T},\ \text{otherwise}\end{matrix}\right.$ (14)
$\exp(\bm{\eta})\triangleq\left\\{\begin{matrix}[\cos(||\bm{\eta}/2||),\sin(||\bm{\eta}/2||)\frac{\bm{\eta}^{T}}{||\bm{\eta}||}]^{T},\
||\bm{\eta}||\neq 0\\\ [1,0,0,0]^{T},\ \text{otherwise}\end{matrix}\right.$
(15)

It can be found analytically that the relations between the rotational
velocity and the derivative of the quaternion logarithm are:

$\bm{\omega}=\bm{J}_{\eta}\dot{\bm{\eta}}$ (16)
$\dot{\bm{\eta}}=\bm{J}^{\dagger}_{\eta}\bm{\omega}$ (17)

where

$\displaystyle\bm{J}_{\eta}$
$\displaystyle\triangleq\bm{k}\bm{k}^{T}+\frac{\sin(\theta_{2})\cos(\theta_{2})}{\theta_{2}}(\bm{I}_{3}-\bm{k}\bm{k}^{T})+\frac{\sin^{2}(\theta_{2})}{\theta_{2}}[\bm{k}]_{\times}$
(18) $\displaystyle\bm{J}^{\dagger}_{\eta}$
$\displaystyle=\bm{k}\bm{k}^{T}+\frac{\theta_{2}\cos(\theta_{2})}{\sin(\theta_{2})}(\bm{I}_{3}-\bm{k}\bm{k}^{T})-\theta_{2}[\bm{k}]_{\times}$
(19)

where $[k]_{times}$ denotes the skew-symmetric matric of $\bm{k}$. For
$\theta=0$ it can be easily verified that taking the limit of (18), (19) and
using L’Hospital’s rule we get
$\bm{J}_{\eta}=\bm{J}^{\dagger}_{\eta}=\bm{I}_{3}$.

Differentiating (16), (17) we can obtain the relations between the quaternion
logarithm second time derivative and the rotational acceleration:

$\dot{\bm{\omega}}=\bm{J}_{\eta}\ddot{\bm{\eta}}+\dot{\bm{J}}_{\eta}\dot{\bm{\eta}}$
(20)
$\ddot{\bm{\eta}}=\bm{J}^{\dagger}_{\eta}\dot{\bm{\omega}}+\dot{\bm{J}}^{\dagger}_{\eta}\bm{\omega}$
(21)

where

$\displaystyle\dot{\bm{J}}_{\eta}$
$\displaystyle=\left(1-\frac{\sin(\theta_{2})\cos(\theta_{2})}{\theta_{2}}\right)(\dot{\bm{k}}\bm{k}^{T}+\bm{k}\dot{\bm{k}}^{T})+\frac{\sin^{2}(\theta_{2})}{\theta_{2}}[\dot{\bm{k}}]_{\times}$
$\displaystyle+\left(\frac{1-2\sin^{2}(\theta_{2})}{\theta_{2}}-\frac{\sin(\theta_{2})\cos(\theta_{2})}{\theta_{2}^{2}}\right)\dot{\theta}_{2}(\bm{I}_{3}-\bm{k}\bm{k}^{T})$
$\displaystyle+\left(\frac{2\sin(\theta_{2})\cos(\theta_{2})}{\theta_{2}}-\frac{\sin^{2}(\theta)}{\theta_{2}^{2}}\right)\dot{\theta}_{2}[\bm{k}]_{\times}$
(22) $\displaystyle\dot{\bm{J}}^{\dagger}_{\eta}$
$\displaystyle=\left(1-\frac{\theta_{2}\cos(\theta_{2})}{\sin(\theta_{2})}\right)(\dot{\bm{k}}\bm{k}^{T}+\bm{k}\dot{\bm{k}}^{T})-\dot{\theta_{2}}[\bm{k}]_{\times}-\theta_{2}[\dot{\bm{k}}]_{\times}$
$\displaystyle+\left(\frac{\sin(\theta_{2})\cos(\theta_{2})-\theta_{2}}{\sin^{2}(\theta_{2})}\right)\dot{\theta}_{2}(\bm{I}_{3}-\bm{k}\bm{k}^{T})$
(23)

with $\dot{\theta}_{2}=\frac{1}{2}\bm{k}^{T}\dot{\bm{\bm{\eta}}}$ and
$\dot{\bm{k}}=\frac{1}{2}\left(\frac{1-\bm{k}\bm{k}^{T}}{\theta_{2}}\dot{\bm{\bm{\eta}}}\right)$.
Taking the limit of (Appendix A - Unit Quaternion Preliminaries), (Appendix A
- Unit Quaternion Preliminaries) for $\theta=0$ we have that
$\dot{\bm{J}}_{\eta}=\dot{\bm{J}}^{\dagger}_{\eta}=\bm{0}$.

Finally, the relations between the Cartesian torque $\bm{\tau}$ and its
transformation in the quaternion logarithm space $\bm{\tau}_{\eta}$ is given
by:

$\bm{\tau}_{\eta}=\bm{J}_{\eta}^{T}\bm{\tau}$ (24)
$\bm{\tau}=(\bm{J}^{\dagger}_{\eta})^{T}\bm{\tau}_{\eta}$ (25)

These mappings follow readily from the preservation of power, i.e. it should
hold that $\bm{\omega}^{T}\bm{\tau}=\dot{\bm{\eta}}^{T}\bm{\tau}_{\eta}$.

## Appendix B - Recursive Least Squares derivations

Here we provide some useful results in the form of theorems which facilitate
the proof of the update formulas (8), (10) that solve (7). Results in Section
2 from [16] for vectors are here extended for matrices in Theorems $1,2$ and
$5$. Theorems $1,2$ are used in the proof of Theorems $3-5$, and Theorems
$2-5$ are employed in the proof for solving (7).

###### Theorem 1

The solution to the problem:

$\displaystyle\text{min}_{\bm{W}}\text{tr}\\{{(\bm{Y}-\bm{W}^{T}\bm{\Phi})^{T}\bm{R}^{-1}(\bm{Y}-\bm{W}^{T}\bm{\Phi})}\\}$
(26)

with $\bm{Y}\in\mathbb{R}^{n\times m}$, $\bm{W}\in\mathbb{R}^{k\times n}$,
$\bm{\Phi}\in\mathbb{R}^{k\times m}$, $\text{rank}(\bm{\Phi})=k$,
$\bm{R}\in\mathcal{S}^{n}_{++}$ is given by:

$\displaystyle\bm{W}_{0}$
$\displaystyle=\bm{P}_{0}\bm{\Phi}\bm{R}^{-1}\bm{Y}^{T}$ (27)
$\displaystyle\bm{P}_{0}$
$\displaystyle=(\bm{\Phi}\bm{R}^{-1}\bm{\Phi}^{T})^{-1}$ (28)

where $\bm{P}_{0}>\bm{0}$.

Proof:  Taking the derivative of $f_{0}$ w.r.t. $\bm{W}$ and solving for
$\bm{W}$, it is straightforward to verify that the solution is indeed given by
$\bm{W}_{0}$, where $\bm{P}>\bm{0}$ since $\bm{R}>\bm{0}$ and
$\text{rank}(\bm{\Phi})=k$.

###### Theorem 2

The solution to the problem:

$\displaystyle\text{min}_{\bm{W}}f_{0}(\bm{W})+\text{tr}\\{{(\bm{Z}-\bm{W}^{T}\bm{H})^{T}\bm{R}_{1}^{-1}(\bm{Z}-\bm{W}^{T}\bm{H})}\\}$
(29)

with
$f_{0}(\bm{W})=\text{tr}\\{{(\bm{Y}-\bm{W}^{T}\bm{\Phi})^{T}\bm{R}^{-1}(\bm{Y}-\bm{W}^{T}\bm{\Phi})}\\}$,
$\bm{Y}\in\mathbb{R}^{n\times m}$, $\bm{W}\in\mathbb{R}^{k\times n}$,
$\bm{\Phi}\in\mathbb{R}^{k\times m}$, $\text{rank}(\bm{\Phi})=k$,
$\bm{R},\bm{R}_{1}\in\mathcal{S}^{n}_{++}$, $\bm{Z}\in\mathbb{R}^{n\times l}$,
$\bm{H}\in\mathbb{R}^{k\times l}$, can be obtained from the solution
$\bm{W}_{0}$, $\bm{P}_{0}$ of $\text{min}_{\bm{W}}f_{0}(\bm{W})$ as follows:

$\displaystyle\bm{W}_{1}$
$\displaystyle=\bm{W}_{0}+\bm{P}_{0}\bm{H}(\bm{R}_{1}+\bm{H}^{T}\bm{P}_{0}\bm{H})^{-1}(\bm{Z}-\bm{W}_{0}^{T}\bm{H})^{T}$
(30) $\displaystyle\bm{P}_{1}$
$\displaystyle=\bm{P}_{0}-\bm{P}_{0}\bm{H}(\bm{R}_{1}+\bm{H}^{T}\bm{P}_{0}\bm{H})^{-1}\bm{H}^{T}\bm{P}_{0}$
(31) $\displaystyle\bm{P}_{1}^{-1}$
$\displaystyle=\bm{P}_{0}+\bm{H}\bm{R}_{1}^{-1}\bm{H}^{T}$ (32)

where $\bm{P}_{1}>\bm{0}$.

Proof:  We can rewritte the cost function in (29) as

$\text{tr}\\{{(\bar{\bm{Y}}-\bm{W}^{T}\bar{\bm{\Phi}})^{T}\bar{\bm{R}}^{-1}(\bar{\bm{Y}}-\bm{W}^{T}\bar{\bm{\Phi}})}\\}$

with

$\bar{\bm{Y}}=[\bm{Y}^{T}\ \bm{Z}^{T}]^{T},\ \bar{\bm{\Phi}}=[\bm{\Phi}^{T}\
\bm{H}^{T}]^{T},\ \bar{\bm{R}}=\text{blkdiag}(\bm{R},\bm{R}_{1})$

and apply the result of Theorem 1 to get:

$\bm{W}=\bm{P}_{1}^{-1}\bar{\bm{\Phi}}\bar{\bm{R}}^{-1}\bar{\bm{Y}}^{T}=\bm{P}_{1}^{-1}(\bm{\Phi}\bm{R}^{-1}\bm{Y}^{T}+\bm{H}\bm{R}_{1}^{-1}\bm{Z}^{T})$
(33)

where
$\bm{P}_{1}^{-1}=\bar{\bm{\Phi}}\bar{\bm{R}}^{-1}\bar{\bm{\Phi}}^{T}=(\bm{P}_{0}^{-1}+\bm{H}\bm{R}_{1}^{-1}\bm{H}^{T})$
with $\bm{P}_{0}^{-1}=\bm{\Phi}\bm{R}^{-1}\bm{\Phi}^{T}$. It follows that
$\bm{P}_{1}^{-1}>\bm{0}$, since $\bm{P}_{0}^{-1}>\bm{0}$, due to
$\bm{R}>\bm{0}$ and $\text{rank}(\bm{\Phi})=k$, and
$\bm{H}\bm{R}_{1}^{-1}\bm{H}^{T}\geq 0$. Applying the matrix inversion lemma
it follows that $\bm{P}_{1}$ is indeed given by (31) and substituting it in
(33) we get:

$\displaystyle\bm{W}=$
$\displaystyle\bm{W}_{0}-\bm{P}_{0}\bm{H}(\bm{R}_{1}+\bm{H}^{T}\bm{P}_{0}\bm{H})^{-1}(\bm{W}_{0}^{T}\bm{H})^{T}+$
(34)
$\displaystyle(\bm{P}_{0}-\bm{P}_{0}\bm{H}(\bm{R}_{1}+\bm{H}^{T}\bm{P}_{0}\bm{H})^{-1}\bm{H}^{T}\bm{P}_{0})\bm{H}\bm{R}_{1}^{-1}\bm{Z}^{T}$

where $\bm{W}_{0}=\bm{P}_{0}\bm{\Phi}\bm{R}^{-1}\bm{Y}^{T}$,
$\bm{P}_{0}=(\bm{\Phi}\bm{R}^{-1}\bm{\Phi}^{T})^{-1}$ is indeed the solution
of $\text{min}_{\bm{W}}f_{0}(\bm{W})$ based on Theorem 1. We can further
process the last term in (34), i.e.:

(35)

and substituting it back to (34) we arrive at the solution given by (30).

###### Theorem 3

Problem (29) is equivalent to the problem

$\displaystyle\text{min}_{\bm{W}}$
$\displaystyle\text{tr}\\{{(\bm{W}-\bm{W}_{0})\bm{P}_{0}^{-1}(\bm{W}-\bm{W}_{0})^{T}}\\}+$
(36)
$\displaystyle\text{tr}\\{{(\bm{Z}-\bm{W}^{T}\bm{H})^{T}\bm{R}_{1}^{-1}(\bm{Z}-\bm{W}^{T}\bm{H})}\\}$
(37)

where $\bm{W}_{0}$, $\bm{P}_{0}$ are the solution to
$\text{min}_{\bm{W}}f_{0}(\bm{W})$.

Proof:  Taking the gradient and solving w.r.t. $\bm{W}$ we can readily obtain
(33) after which the same analysis as in the proof of Theorem 2 follows.

###### Theorem 4

The optimization problem

$\displaystyle\text{min}_{\bm{W}}$ $\displaystyle f_{0}(\bm{W})$ (38) s.t.
$\displaystyle\bm{W}^{T}\bm{H}=\bm{Z}$

with
$f_{0}(\bm{W})=\text{tr}\\{{(\bm{Y}-\bm{W}^{T}\bm{\Phi})^{T}\bm{R}^{-1}(\bm{Y}-\bm{W}^{T}\bm{\Phi})}\\}$,
$\bm{Y}\in\mathbb{R}^{n\times m}$, $\bm{W}\in\mathbb{R}^{k\times n}$,
$\bm{\Phi}\in\mathbb{R}^{k\times m}$, $\text{rank}(\bm{\Phi})=k$,
$\bm{R}\in\mathcal{S}^{n}_{++}$, $\bm{Z}\in\mathbb{R}^{n\times l}$,
$\bm{H}\in\mathbb{R}^{k\times l}$, $\text{rank}(\bm{H})=l\leq k$ , is
equivalent to the problem:

$\text{min}_{\bm{W}}f_{0}(\bm{W})+\text{tr}\\{{(\bm{Z}-\bm{W}^{T}\bm{H})^{T}\bm{R}_{\epsilon}^{-1}(\bm{Z}-\bm{W}^{T}\bm{H})}\\}$
(39)

for $\bm{R}_{\epsilon}\rightarrow\bm{0}^{+}$.

Proof:  To find the solution of (38) we introduce the Language multipliers
$\bm{V}\in\mathbb{R}^{n\times l}$ and form the Lagrangian
$L(\bm{W},\bm{V})=f_{0}(\bm{W})+2\text{tr}\\{{\bm{V}^{T}(\bm{W}^{T}\bm{H}-\bm{Z})}\\}$.
The KKT conditions are:

$\displaystyle\frac{\partial
L}{\partial\bm{W}}=-\bm{\Phi}(\bm{Y}-\bm{W}^{T}\bm{\Phi})^{T}+\bm{H}\bm{V}=\bm{0}$
(40) $\displaystyle\frac{\partial
L}{\partial\bm{V}}=\bm{H}^{T}\bm{W}-\bm{Z}^{T}=\bm{0}$ (41)

Solving (40) for $\bm{W}$ we get:

$\bm{W}=\bm{W}_{0}-\bm{P}_{0}\bm{H}\bm{V}$ (42)

where $\bm{W}_{0}=\bm{P}_{0}\bm{\Phi}\bm{Y}^{T}$,
$\bm{P}_{0}=(\bm{\Phi}\bm{R}^{-1}\bm{\Phi}^{T})^{-1}$ which is indeed
invertible since $\text{rank}(\bm{\Phi})=k$. Since $\bm{P}_{0}$ is invertible
and $\text{rank}(\bm{H})=l$, i.e. has full column rank, we can substitute (42)
in (41) to solve for $\bm{V}$:

$\bm{V}=-(\bm{H}^{T}\bm{P}_{0}\bm{H})^{-1}(\bm{Z}-\bm{W}_{0}^{T}\bm{H})^{T}$

Substituting the last equation in (42), we obtain the solution:

$\bm{W}_{1}=\bm{W}_{0}+\bm{P}_{0}\bm{H}(\bm{H}^{T}\bm{P}_{0}\bm{H})^{-1}(\bm{Z}-\bm{W}_{0}^{T}\bm{H})^{T}$
(43)

Turning our attention to problem (39) we can apply the result of Theorem 2 and
notice that for $\bm{R}_{1}=\bm{R}_{\epsilon}\rightarrow\bm{0}^{+}$ we get
indeed the same solution as in (43).

###### Theorem 5

Given the solution $\bm{W}_{1}$, $\bm{P}_{1}$ of the problem

$\text{min}_{\bm{W}}f_{0}(\bm{W})+\text{tr}\\{{(\bm{Z}-\bm{W}^{T}\bm{H})^{T}\bm{R}_{1}^{-1}(\bm{Z}-\bm{W}^{T}\bm{H})}\\}$
(44)

with
$f_{0}(\bm{W})=\text{tr}\\{{(\bm{Y}-\bm{W}^{T}\bm{\Phi})^{T}\bm{R}^{-1}(\bm{Y}-\bm{W}^{T}\bm{\Phi})}\\}$,
$\bm{Y}\in\mathbb{R}^{n\times m}$, $\bm{W}\in\mathbb{R}^{k\times n}$,
$\bm{\Phi}\in\mathbb{R}^{k\times m}$, $\text{rank}(\bm{\Phi})=k$,
$\bm{R},\bm{R}_{1}\in\mathcal{S}^{n}_{++}$, $\bm{Z}\in\mathbb{R}^{n\times l}$,
$\bm{H}\in\mathbb{R}^{k\times l}$, the solution to the problem:

$\text{min}_{\bm{W}}f_{0}(\bm{W})$ (45)

can be calculated as:

$\displaystyle\hat{\bm{W}}_{0}$
$\displaystyle=\bm{W}_{1}+\bm{P}_{1}\bm{H}(-\bm{R}_{1}+\bm{H}^{T}\bm{P}_{1}\bm{H})^{-1}(\bm{Z}-\bm{W}_{1}^{T}\bm{H})^{T}$
(46) $\displaystyle\hat{\bm{P}}_{0}$
$\displaystyle=\bm{P}_{1}-\bm{P}_{1}\bm{H}(-\bm{R}_{1}+\bm{H}^{T}\bm{P}_{1}\bm{H})^{-1}\bm{H}^{T}\bm{P}_{1}$
(47) $\displaystyle\hat{\bm{P}}_{0}^{-1}$
$\displaystyle=\bm{P}_{1}^{-1}-\bm{H}\bm{R}_{1}^{-1}\bm{H}^{T}$ (48)

where $\hat{\bm{P}}_{0}>\bm{0}$.

Proof:  Denoting

$f_{1}(\bm{W})=f_{0}(\bm{W})+\text{tr}\\{{(\bm{Z}-\bm{W}^{T}\bm{H})^{T}\bm{R}_{1}^{-1}(\bm{Z}-\bm{W}^{T}\bm{H})}\\}$

it follows that

$f_{0}(\bm{W})=f_{1}(\bm{W})-\text{tr}\\{{(\bm{Z}-\bm{W}^{T}\bm{H})^{T}\bm{R}_{1}^{-1}(\bm{Z}-\bm{W}^{T}\bm{H})}\\}$

We can rewrite $f_{1}(\bm{W})$ as:

$f_{1}(\bm{W})=\text{tr}\\{{(\bar{\bm{Y}}-\bm{W}^{T}\bar{\bm{\Phi}})^{T}\bar{\bm{R}}^{-1}(\bar{\bm{Y}}-\bm{W}^{T}\bar{\bm{\Phi}})}\\}$

with

$\bar{\bm{Y}}=[\bm{Y}^{T}\ \bm{Z}^{T}]^{T},\ \bar{\bm{\Phi}}=[\bm{\Phi}^{T}\
\bm{H}^{T}]^{T},\ \bar{\bm{R}}=\text{blkdiag}(\bm{R},\bm{R}_{1})$

Minimizing $f_{0}(\bm{W})$ we have:

$\displaystyle\frac{\partial
f_{0}}{\partial\bm{W}}=\frac{\partial\left(\scalebox{0.9}{\mbox{$\displaystyle
f_{1}(\bm{W})-\text{tr}\\{{(\bm{Z}-\bm{W}^{T}\bm{H})^{T}\bm{R}_{1}^{-1}(\bm{Z}-\bm{W}^{T}\bm{H})}\\}$}}\right)}{\partial\bm{W}}=\bm{0}$
$\displaystyle(\bm{P}_{1}^{-1}-\bm{H}\bm{R}_{1}^{-1}\bm{H}^{T})\bm{W}=(\bar{\bm{\Phi}}\bar{\bm{R}}^{-1}\bar{\bm{Y}}^{T}-\bm{H}\bm{R}_{1}^{-1}\bm{Z}^{T})$
(49)

where
$\bm{P}_{1}^{-1}=\bar{\bm{\Phi}}\bar{\bm{R}}^{-1}\bar{\bm{\Phi}}^{T}=\bm{P}_{0}^{-1}+\bm{H}\bm{R}_{1}^{-1}\bm{H}^{T}$
and $\bm{P}_{0}^{-1}=\bm{\Phi}\bm{R}^{-1}\bm{\Phi}^{T}$. Since $\bm{R}>\bm{0}$
and $\text{rank}(\bm{\Phi})=k$ it follows that $\bm{P}_{0}^{-1}>\bm{0}$ which
also entails that $\bm{P}_{1}^{-1}>\bm{0}$ as $\bm{R}_{1}>\bm{0}$ and
$\bm{H}\bm{R}_{1}^{-1}\bm{H}^{T}\geq\bm{0}$. Notice also that
$(\bm{P}_{1}^{-1}-\bm{H}\bm{R}_{1}^{-1}\bm{H}^{T})=\bm{P}_{0}^{-1}$ hence we
can invert it in (49) to get:

$\displaystyle\bm{W}=(\bm{P}_{1}^{-1}-\bm{H}\bm{R}_{1}^{-1}\bm{H}^{T})^{-1}(\bar{\bm{\Phi}}\bar{\bm{R}}^{-1}\bar{\bm{Y}}^{T}+\bm{H}(-\bm{R}_{1}^{-1})\bm{Z}^{T})$

Applying the matrix inversion lemma we have:

$\displaystyle\bm{W}=$
$\displaystyle\bm{W}_{1}-\bm{P}_{1}\bm{H}(-\bm{R}_{1}+\bm{H}^{T}\bm{P}_{1}\bm{H})^{-1}(\bm{W}_{1}^{T}\bm{H})^{T}+$
(50)
$\displaystyle(\bm{P}_{1}-\bm{P}_{1}\bm{H}(-\bm{R}_{1}+\bm{H}^{T}\bm{P}_{1}\bm{H})^{-1}\bm{H}^{T}\bm{P}_{1})\bm{H}(-\bm{R}_{1}^{-1})\bm{Z}^{T}$

where $\bm{W}_{1}=\bm{P}_{1}\bar{\bm{\Phi}}\bar{\bm{R}}^{-1}\bar{\bm{Y}}^{T}$,
$\bm{P}_{1}=(\bar{\bm{\Phi}}\bar{\bm{R}}^{-1}\bar{\bm{\Phi}}^{T})^{-1}$ is
indeed the solution of $\text{min}_{\bm{W}}f_{1}(\bm{W})$ based on Theorem 1.
Notice that $-\bm{R}_{1}+\bm{H}^{T}\bm{P}_{1}\bm{H}$ is in fact negative
definite, hence it is invertible. To prove so, consider the following matrix:

$\bm{M}=\begin{bmatrix}\bm{P}_{1}^{-1}&\bm{H}\\\
\bm{H}^{T}&\bm{R}_{1}\end{bmatrix}$

Since $\bm{P}_{1}^{-1}>\bm{0}$ and the Schur complement of $\bm{M}$ w.r.t. the
upper left block matrix is
$\bm{P}_{1}^{-1}-\bm{H}\bm{R}_{1}^{-1}\bm{H}^{T}=\bm{P}_{0}^{-1}>\bm{0}$ it
follows that $\bm{M}>\bm{0}$ [17]. Therefore, as $\bm{R}_{1}>\bm{0}$ and
$\bm{M}>\bm{0}$ it should also hold that the Schur complement of $\bm{M}$
w.r.t. the lower right block matrix should be positive definite, i.e.:

$\bm{R}_{1}^{-1}-\bm{H}^{T}\bm{P}_{1}\bm{H}>\bm{0}\Rightarrow-\bm{R}_{1}^{-1}+\bm{H}^{T}\bm{P}_{1}\bm{H}<\bm{0}$

We can further process the last term in (50) similar to the analysis in
(Appendix B - Recursive Least Squares derivations) to find that:

$\displaystyle(\bm{P}_{1}-\bm{P}_{1}\bm{H}(-\bm{R}_{1}+\bm{H}^{T}\bm{P}_{1}\bm{H})^{-1}\bm{H}^{T}\bm{P}_{1})\bm{H}(-\bm{R}_{1}^{-1})\bm{Z}^{T}=$
$\displaystyle\bm{P}_{1}\bm{H}(-\bm{R}_{1}+\bm{H}^{T}\bm{P}_{1}\bm{H})^{-1}\bm{Z}^{T}$

and substituting the last in (50) we obtain the result given by (46). Finally,
having already established that $\bm{P}_{1}>\bm{0}$ and
$-\bm{R}_{1}^{-1}+\bm{H}^{T}\bm{P}_{1}\bm{H}<\bm{0}$ it follows that
$-\bm{P}_{1}\bm{H}(-\bm{R}_{1}+\bm{H}^{T}\bm{P}_{1}\bm{H})^{-1}\bm{H}^{T}\bm{P}_{1}\geq\bm{0}$
hence $\hat{\bm{P}}_{0}>\bm{0}$.

## References

  * [1] A. Billard, S. Calinon, R. Dillmann, and S. Schaal, _Robot Programming by Demonstration_. Berlin, Heidelberg: Springer Berlin Heidelberg, 2008, pp. 1371–1394.
  * [2] A. J. Ijspeert, J. Nakanishi, H. Hoffmann, P. Pastor, and S. Schaal, “Dynamical movement primitives: Learning attractor models for motor behaviors,” _Neural Comput._ , vol. 25, no. 2, pp. 328–373, Feb 2013.
  * [3] L. Koutras and Z. Doulgeri, “A novel dmp formulation for global and frame independent spatial scaling in the task space,” in _2020 29th IEEE International Conference on Robot and Human Interactive Communication (RO-MAN)_ , 2020, pp. 727–732.
  * [4] H. Hoffmann, P. Pastor, D. Park, and S. Schaal, “Biologically-inspired dynamical systems for movement generation: Automatic real-time goal adaptation and obstacle avoidance,” in _2009 IEEE International Conference on Robotics and Automation_ , 2009, pp. 2587–2592.
  * [5] G. Maeda, M. Ewerton, R. Lioutikov, H. B. Amor, J. Peters, and G. Neumann, “Learning interaction for collaborative tasks with probabilistic movement primitives,” in _2014 IEEE-RAS International Conference on Humanoid Robots_ , Nov 2014, pp. 527–534.
  * [6] G. J. Maeda, G. Neumann, M. Ewerton, R. Lioutikov, O. Kroemer, and J. Peters, “Probabilistic movement primitives for coordination of multiple human–robot collaborative tasks,” _Autonomous Robots_ , vol. 41, no. 3, pp. 593–612, Mar 2017. [Online]. Available: https://doi.org/10.1007/s10514-016-9556-2
  * [7] C. Cardoso, L. Jamone, and A. Bernardino, “A novel approach to dynamic movement imitation based on quadratic programming,” in _2015 IEEE International Conference on Robotics and Automation (ICRA)_ , 2015, pp. 906–911.
  * [8] A. Paraschos, C. Daniel, J. Peters, and G. Neumann, “Using probabilistic movement primitives in robotics,” _Auton. Robots_ , vol. 42, no. 3, p. 529–551, Mar. 2018. [Online]. Available: https://doi.org/10.1007/s10514-017-9648-7
  * [9] P. Pastor, L. Righetti, M. Kalakrishnan, and S. Schaal, “Online movement adaptation based on previous sensor experiences,” in _2011 IEEE/RSJ International Conference on Intelligent Robots and Systems_ , Sept 2011, pp. 365–371.
  * [10] A. Gams, A. Ijspeert, S. Schaal, and J. Lenarcic, “On-line learning and modulation of periodic movements with nonlinear dynamical systems,” _Autonomous Robots_ , vol. 27, pp. 3–23, 07 2009.
  * [11] J. Umlauft, D. Sieber, and S. Hirche, “Dynamic movement primitives for cooperative manipulation and synchronized motions,” in _2014 IEEE International Conference on Robotics and Automation (ICRA)_ , 2014, pp. 766–771.
  * [12] B. Nemec, A. Gams, M. Deniša, and A. Ude, “Human-robot cooperation through force adaptation using dynamic motion primitives and iterative learning,” in _2014 IEEE International Conference on Robotics and Biomimetics (ROBIO 2014)_ , 2014, pp. 1439–1444.
  * [13] A. Sidiropoulos and Z. Doulgeri, “A reversible dynamic movement primitive formulation,” in _2021 International Conference on Robotics and Automation (ICRA)_ , 2021, pp. ?–?
  * [14] A. Sidiropoulos, D. Papageorgiou, and Z. Doulgeri, “A novel framework for generalizing dynamic movement primitives under kinematic constraints,” _Autonomous Robots_ , Oct 2022. [Online]. Available: https://doi.org/10.1007/s10514-022-10067-4
  * [15] T. Kastritsi, D. Papageorgiou, I. Sarantopoulos, Z. Doulgeri, and G. A. Rovithakis, “Stability of active constraints enforcement in sensitive regions defined by point-clouds for robotic surgical procedures,” in _2019 18th European Control Conference (ECC)_ , 2019, pp. 1604–1609.
  * [16] T. Kailath, A. Sayed, and B. Hassibi, _Linear Estimation_ , ser. Prentice-Hall information and system sciences series. Prentice Hall, 2000. [Online]. Available: https://books.google.gr/books?id=zNJFAQAAIAAJ
  * [17] G. Strang, _Linear algebra and its applications_. Belmont, CA: Thomson, Brooks/Cole, 2006.