added a simple solution for the drawbacks of the modified DMP

chauby · Mar 22, 2023 · 6c1496d · 6c1496d
1 parent 5d45474
commit 6c1496d
Show file tree

Hide file tree

Showing 10 changed files with 28 additions and 141 deletions.
diff --git a/assets/image-20230322222549918.png b/assets/image-20230322222549918.png
diff --git a/assets/image-20230322223132548.png b/assets/image-20230322223132548.png
diff --git a/assets/image-20230322223318180.png b/assets/image-20230322223318180.png
diff --git a/code/DMP/dmp_discrete.py b/code/DMP/dmp_discrete.py
@@ -213,9 +213,14 @@ def step(self, tau=None):
 
             # ---------- Modified DMP in Schaal 2008, fixed the problem of g-y_0 -> 0
             k = self.alpha_y[d]
-            # f = k*(np.dot(psi, self.w[d])*x / np.sum(psi)) - k*(self.goal[d] - self.y0[d])*x
-            self.delta_2[d] = self.goal[d] - self.y0[d]
-            f = k*(np.dot(psi, self.w[d])*x*(self.delta_2[d]/self.delta[d]) / np.sum(psi)) - k*(self.goal[d] - self.y0[d])*x
+            # f = k*(np.dot(psi, self.w[d])*x / np.sum(psi)) - k*(self.goal[d] - self.y0[d])*x # Modified DMP
+
+            self.delta_2[d] = self.goal[d] - self.y0[d] # Modified DMP extended
+            if abs(self.delta[d]) > 1e-5:
+                k2 = self.delta_2[d]/self.delta[d]
+            else:
+                k2 = 1.0
+            f = k*(np.dot(psi, self.w[d])*x*k2 / np.sum(psi)) - k*(self.goal[d] - self.y0[d])*x
 
             # generate reproduced trajectory
             self.ddy[d] = self.alpha_y[d]*(self.beta_y[d]*(self.goal[d] - self.y[d]) - self.dy[d]) + f

diff --git a/code/DMP/dmp_discrete_classic.py → code/DMP/dmp_discrete_original.py b/code/DMP/dmp_discrete_classic.py → code/DMP/dmp_discrete_original.py
diff --git a/code/DMP/test.py b/code/DMP/test.py
diff --git a/code/DMP/test_3.py b/code/DMP/test_3.py
diff --git a/code/demo_trajectory/LKnee_arr_gaits.npz b/code/demo_trajectory/LKnee_arr_gaits.npz
diff --git a/code/DMP/test_2.py → code/test_modified_discrete_DMP.py b/code/DMP/test_2.py → code/test_modified_discrete_DMP.py
@@ -8,6 +8,9 @@
 import numpy as np
 import matplotlib.pyplot as plt
 
+import sys
+sys.path.append('DMP')
+
 from dmp_discrete import dmp_discrete
 
 # %%
@@ -25,7 +28,7 @@
 y_demo[1,:] = demo_traj[1,:]
 
 # DMP learning
-dmp = dmp_discrete(n_dmps=y_demo.shape[0], n_bfs=200, dt=1.0/data_len, alpha_y=50, beta_y = 10)
+dmp = dmp_discrete(n_dmps=y_demo.shape[0], n_bfs=200, dt=1.0/data_len)
 dmp.learning(y_demo, plot=False)
 
 # reproduce learned trajectory

diff --git a/solution for the drawbacks of modified DMP.md b/solution for the drawbacks of modified DMP.md
@@ -0,0 +1,16 @@
+改进型的DMP算法可能存在更改轨迹初始点或目标点以后轨迹形状畸变的问题：
+
+![image-20230322222549918](assets/image-20230322222549918.png)
+
+这是由于改进型的DMP算法为了避免轨迹起始点和终止点相同时非线性项$f$无法计算的问题，从而将$g-y_0$与$f$分离了，具体公式详见我知乎的博客。这样就会导致非线性项无法感知轨迹的起始点/目标点与原始示教轨迹起始点/目标点之间的差异，从而无法建模这些变化，导致复现后的轨迹没问题，而改变起始点/目标点以后的轨迹形状发生畸变的问题。
+
+最简单直接的办法就是将这部分再考虑到轨迹建模当中，因此可以使用某种trick将轨迹变化的差异建模到非线性项$f$当中，比如将新的目标点与起始点之间的距离、旧的目标点与起始点之间的距离这两者之间计算一个比例，然后在非线性项$f$中进行放缩：
+
+![image-20230322223132548](assets/image-20230322223132548.png)
+
+这样就可以在某些场景下简单的解决这个问题，在观感上不至于差别太大，但是对轨迹数据敏感，很容易引起其他问题，因此无法解决所有情况下的轨迹畸变问题。
+
+![image-20230322223318180](assets/image-20230322223318180.png)
+
+最好的办法已经放在我的知乎博客中了，另一个作者提出的方法，对非线性项的基函数进行了重新设计。
+