# DMP-matlab

**Repository Path**: chauby/DMP-matlab

## Basic Information

- **Project Name**: DMP-matlab
- **Description**: Matlab语言实现的DMP, 包括Dynamic System。
- **Primary Language**: Matlab
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 5
- **Forks**: 0
- **Created**: 2018-04-10
- **Last Updated**: 2024-05-23

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

Dynamic Movement Primitives for matlab
===
by chauby Zou

2018.04.09


# 1. 简介
本代码主要是 Dynamic Movement Primitives(DMPs) 的matlab代码实现, DMPs又称为动态运动基元，基本思想是将运动过程分解为一系列的运动基元，通过运动基元的加权叠加来得到运动轨迹。
# 2. Dynamic System (动力系统、动态系统)
## 2.1 一阶动力系统
微分方程表示为
$$
\dot{x} = -\alpha x
$$
给定目标位置的微分方程表示为
$$
\begin{align}
\dot{x} &= -\alpha (x-x_g)\\
 &= \alpha (x_g - x)
 \end{align}
$$
其中，$\alpha$ 为衰减率（时间常数）, $x_g$ 为目标状态。
<br>
一阶动态系统的时间响应示意图如下

<div align="center">
<img src="Picture/demoFirstOrderDS.png" height="500" width="500">
</div>

## 2.2 二阶动力系统
二阶动力系统其实就是弹簧-阻尼系统，示意图如下所示：

<div align="center">
<img src="Picture/SpringDamperSystem.png" height="400" width="550">
</div>

其微分方程表示为
$$
m\ddot{x} = -kx-c\dot{x}
$$
其中，$m$ 为模块的质量，$k$ 为弹簧系数，$c$ 为阻尼系数。

给定目标位置的微分方程推导过程为
$$
\begin{align}
m\ddot{y} &= -ky-c\dot{y} \\
m\ddot{y} &= c(-\frac{k}{c}y-\dot{y}) \\
\tau\ddot{y} &= \alpha(-\beta y -\dot{y}) \\
\tau\ddot{y} &= \alpha(-\beta (y - y^g) -\dot{y}) \\
\tau\ddot{y} &= \alpha(\beta (y^g - y) -\dot{y}) \\
\end{align}
$$

其中，$\alpha=c, \beta=\frac{k}{c}, m=\tau$, $y^g$ 为设定的目标状态。二阶动态系统的时间响应示意图如下所示：

<div align="center">
<img src="Picture/demoSecondOrderDS.png" height="500" width="500">
</div>

其中，$\tau=1, \alpha=1,\beta=\frac{\alpha}{4}=0.25$, $y, dy, ddy$ 分别是曲线的位置、速度和加速度信息。

<div align="center">
<img src="Picture/demoSecondOrderDS2.png" height="500" width="500">
</div>

其中，$\tau=1$, $\alpha$ 和 $\beta$ 的值如图中所示。

# 3. Dynamic Movement Primitives
动态运动基元(Dynamic Movement Primitives, DMPs)是基于动态系统的一种轨迹调制方法，通过将动作分解为一系列的基元，并使用运动基元加权的方式来实现轨迹的拟合。DMP分为离散DMP(discrete DMP)和节律DMP（rhythmicity DMP）。

## 3.1 离散DMP
其微分方程可以表示为

$$
\tau \ddot{y} = \alpha(\beta(g-y) - \dot{y}) + f
$$

其中 $f$ 是一个非线性函数，表示外界对于动态系统的一个非线性“力”， $f$ 可以表示为

$$
f(x, g) = \frac{\sum^{N}_{i=1}\psi_i w_i}{\sum^{N}_{i=1}\psi_i}x(g-y_0)
$$

其中，$x$ 是一个典型动态系统(canonical dynamical system), 其微分方程可以表示为
$$
\dot{x} = -\alpha_x x
$$

$y_0$ 表示系统的初始值。 $w_i$ 表示权值， $\psi_i$ 表示第 $i$ 个中心值在 $c_i$的高斯函数：
$$
\psi_i=exp(-h_i(x-c_i)^2)
$$

# 4. Application