This repository contains the implementation of Dynamic Movement Primitives, in Python 3.5.
In particular, this repository contains all the synthetic tests done for the work
M. Ginesi, N. Sansonetto, and P. Fiorini; Overcoming Some Drawbacks of Dynamic Movement Primitives
You can find the pre-print at https://arxiv.org/abs/1908.10608
The package can be installed by running
pip install -e .
or
pip3 install -e .
After installation, you can import the DMP class as
from dmp.dmp_cartesian import DMPs_cartesian as dmp
After importing, you can create a class using
MP = dmp()
You can personalize the parameters using keywoard arguments, use the help for additional details. After importing, you can create a trajectory and learning it.
t = np.linspace(0, np.pi, 100)
X = np.transpose(np.array([t * np.cos(2 * t), t * np.sin(2 * t), t * t]))
MP.imitate_path(X)
Finally, you can execute a DMP using
x_track, _, _, _ = MP.rollout()
See the demos/ folder to see scripts in which various options are tested and compared, as well as how to change start and goal position
This repository contains two folders, namely dmp/ and demos/. The dmp/ folder contains all the functions needed to implement DMPs, while the demo/ folder contains the scripts used to perform the tests presented on the paper.
dmp/ contain the following files:
- cs.py implements the Canonical System class, together with its methods.
-
derivative_matrices.py implements the following two functions:
-
compute_D1(n, dt)
returns the matrix which discretize the first derivative of a 1D function discretized on an equispaced time domain ofn
points anddt
timestep, using a second orde estimate; -
compute_D2(n, dt)
returns the matrix which discretize the second derivative of a 1D function discretized on an equispaced time domain ofn
points anddt
timestep, using a second orde estimate.
-
- dmp_cartesian.py implements the DMP class, together with its methods.
-
exponential_integrator.py implements the functions needed to perform an integration step using the Exponential Euler method. In particular the function
exp_eul_step(y, A, b, dt)
returns the solution at time $ n+1$, computed as $ y_{n+1} = y_n + k \varphi_1(k A) (A y_n + b(t_n)) $ for the problem $ \dot{y} = A y + b(t) $, with$y_n$ =y
,$A$ =A
,$b(t_n)$ =b
, and$k$ =dt
. -
rotation_matri.py implements the functions needed to compute the roto-dilatation matrix. In particular,
roto_dilatation(x0, x1)
returns the roto-dilatation matrix which mapsx0
tox1
.
demos/ contain the following files:
- demo_basis.py tests the accuracy in the approximation of a given function using different types of basis functions (Gaussian, trucated Gaussians, Wendland, and Mollifier-like). See Figure 2 of the paper.
- demo_regression.py tests the learning-from-multible-observations porcess using a set of trajecotries obtained by integrating a known dynamical system. See Figure 4a-4b of the paper.
- demo_rescaling.py tests on the robustness of DMP against modification of the relative position between starting and ending points, with and without exploiting the invariance property. See Figure 3 of the paper.
- demo_rescaling_1.py additional tests on the robustness of DMP against modification of the relative position between starting and ending points, with and without exploiting the invariance property.
The term Dynamic Movement Primitives (DMPs) refers to a framework for trajectory learning based on second order ODE of spring-mass-damping type:
$$ \begin{cases}
\tau \dot{\mathbf{v}} = \mathbf{K} (\mathbf{g} - \mathbf{x}) - \mathbf{D} \mathbf{v} - \mathbf{K} ( \mathbf{g} - \mathbf{x}_0 ) s + \mathbf{K} \mathbf{f}(s) \
\tau \dot{\mathbf{x}} = \mathbf{v}
\end{cases} , $$
where
Forcing term
We extend the approach to multiple set of basis functions. In particular, we propose to use various classes of Wendland's basis functions
$$ \begin{aligned}
\phi_i^{(2)} (s) & = (1 - r)^2_+ \
\phi_i^{(3)} (s) & = (1 - r)^3_+ \
\phi_i^{(4)} (s) & = (1 - r)^4_+ (4r + 1) \
\phi_i^{(5)} (s) & = (1 - r)^5_+ (5r + 1) \
\phi_i^{(6)} (s) & = (1 - r)^6_+ (35 r ^ 2 + 18 r + 3) \
\phi_i^{(7)} (s) & = (1 - r)^7_+ (16 r ^ 2 + 7 r + 1) \
\phi_i^{(8)} (s) & = (1 - r)^8_+ (32 r ^ 3 + 25 r^2 + 8 r + 1) \
\end{aligned}$$
where
These basis functions are plotted in the figure. The first is the Gaussian functions, the second the mollifier, the others are the Wendland. All of them are plotted using
During the learning phase, a trajectory
Then, the dynamical system can be integrated using the same weights when defining the forcing term, but possibly changing the initial and goal position. This will result in a trajectory of similar shape to the learned one, but adapted to the new points. Moreover, the goal position can change during the execution and convergence to it is still guaranteed.
DMPs can be written to be invariant under affine transformations. We have implemented this property in the particular case of roto-dilatation.
In the following figure, the desired (and learned) trajectory is plotted in blue. the new goal is represented by the black star. The dashed red line shows the execution obtained with classical DMPs, while the dash-dotted green line show the execution obtained by taking advantage of the affine invariance.
The "original" DMP formulation was slightly different:
It presented some drawbacks when the learning quantities
- if
$g - x_0$ is zero, the forcing term can not be learned; - if
$g - x_0$ is small and it changes between learning and execution, the trajectory can become extremely different - if
$g - x_0$ change sign from the learned to the desired configuration, the trajectory will result mirrored. The second and third drawbacks are shown in the figure below (you can reproduce the result by runningdemos/old_vs_new.py
).
To contact me please use one of the following mail addresses: