class dmp_discrete(): def __init__(self, n_dmps=1, n_bfs=100, dt=0, alpha_y=None, beta_y=None, **kwargs): self.n_dmps = n_dmps # number of data dimensions, one dmp for one degree self.n_bfs = n_bfs # number of basis functions self.dt = dt self.y0 = np.zeros(n_dmps) # for multiple dimensions self.goal = np.ones(n_dmps) # for multiple dimensions self.alpha_y = np.ones(n_dmps) * 60.0 if alpha_y is None else alpha_y self.beta_y = self.alpha_y / 4.0 if beta_y is None else beta_y self.tau = 1.0 self.w = np.zeros((n_dmps, n_bfs)) # weights for forcing term self.psi_centers = np.zeros( self.n_bfs ) # centers over canonical system for Gaussian basis functions self.psi_h = np.zeros( self.n_bfs ) # variance over canonical system for Gaussian basis functions # canonical system self.cs = CanonicalSystem(dt=self.dt, **kwargs) self.timesteps = round(self.cs.run_time / self.dt) # generate centers for Gaussian basis functions self.generate_centers() # self.h = np.ones(self.n_bfs) * self.n_bfs / self.psi_centers # original self.h = np.ones( self.n_bfs ) * self.n_bfs**1.5 / self.psi_centers / self.cs.alpha_x # chose from trail and error # reset state self.reset_state() # Reset the system state def reset_state(self): self.y = self.y0.copy() self.dy = np.zeros(self.n_dmps) self.ddy = np.zeros(self.n_dmps) self.cs.reset_state() def generate_centers(self): t_centers = np.linspace(0, self.cs.run_time, self.n_bfs) # centers over time cs = self.cs x_track = cs.run() # get all x over run time t_track = np.linspace(0, cs.run_time, cs.timesteps) # get all time ticks over run time for n in range(len(t_centers)): for i, t in enumerate(t_track): if abs( t_centers[n] - t ) <= cs.dt: # find the x center corresponding to the time center self.psi_centers[n] = x_track[i] return self.psi_centers def generate_psi(self, x): if isinstance(x, np.ndarray): x = x[:, None] self.psi = np.exp(-self.h * (x - self.psi_centers)**2) return self.psi def generate_weights(self, f_target): x_track = self.cs.run() psi_track = self.generate_psi(x_track) for d in range(self.n_dmps): # ------------ Original DMP in Schaal 2002 # delta = self.goal[d] - self.y0[d] # ------------ Modified DMP in Schaal 2008 delta = 1.0 for b in range(self.n_bfs): # as both number and denom has x(g-y_0) term, thus we can simplify the calculation process numer = np.sum(x_track * psi_track[:, b] * f_target[:, d]) denom = np.sum(x_track**2 * psi_track[:, b]) # numer = np.sum(psi_track[:,b] * f_target[:,d]) # the simpler calculation # denom = np.sum(x_track * psi_track[:,b]) self.w[d, b] = numer / (denom * delta) return self.w def learning(self, y_demo, plot=False): if y_demo.ndim == 1: # data is with only one dimension y_demo = y_demo.reshape(1, len(y_demo)) self.y0 = y_demo[:, 0].copy() self.goal = y_demo[:, -1].copy() self.y_demo = y_demo.copy() # interpolate the demonstrated trajectory to be the same length with timesteps x = np.linspace(0, self.cs.run_time, y_demo.shape[1]) y = np.zeros((self.n_dmps, self.timesteps)) for d in range(self.n_dmps): y_tmp = interp1d(x, y_demo[d]) for t in range(self.timesteps): y[d, t] = y_tmp(t * self.dt) # calculate velocity and acceleration of y_demo # method 1: using gradient dy_demo = np.gradient(y, axis=1) / self.dt ddy_demo = np.gradient(dy_demo, axis=1) / self.dt # method 2: using diff # dy_demo = np.diff(y) / self.dt # # let the first gradient same as the second gradient # dy_demo = np.hstack((np.zeros((self.n_dmps, 1)), dy_demo)) # Not sure if is it a bug? # # dy_demo = np.hstack((dy_demo[:,0].reshape(self.n_dmps, 1), dy_demo)) # ddy_demo = np.diff(dy_demo) / self.dt # # let the first gradient same as the second gradient # ddy_demo = np.hstack((np.zeros((self.n_dmps, 1)), ddy_demo)) # # ddy_demo = np.hstack((ddy_demo[:,0].reshape(self.n_dmps, 1), ddy_demo)) x_track = self.cs.run() f_target = np.zeros((y_demo.shape[1], self.n_dmps)) for d in range(self.n_dmps): # ---------- Original DMP in Schaal 2002 # f_target[:,d] = ddy_demo[d] - self.alpha_y[d]*(self.beta_y[d]*(self.goal[d] - y_demo[d]) - dy_demo[d]) # ---------- Modified DMP in Schaal 2008, fixed the problem of g-y_0 -> 0 k = self.alpha_y[d] f_target[:, d] = (ddy_demo[d] - self.alpha_y[d] * (self.beta_y[d] * (self.goal[d] - y_demo[d]) - dy_demo[d]) ) / k + x_track * (self.goal[d] - self.y0[d]) self.generate_weights(f_target) if plot is True: # plot the basis function activations plt.figure() plt.subplot(211) psi_track = self.generate_psi(self.cs.run()) plt.plot(psi_track) plt.title('basis functions') # plot the desired forcing function vs approx plt.subplot(212) plt.plot(f_target[:, 0]) plt.plot(np.sum(psi_track * self.w[0], axis=1) * self.dt) plt.legend(['f_target', 'w*psi']) plt.title('DMP forcing function') plt.tight_layout() plt.show() # reset state self.reset_state() def reproduce(self, tau=None, initial=None, goal=None): # set temporal scaling if tau == None: timesteps = self.timesteps else: timesteps = round(self.timesteps / tau) # set initial state if initial != None: self.y0 = initial # set goal state if goal != None: self.goal = goal # reset state self.reset_state() y_reproduce = np.zeros((timesteps, self.n_dmps)) dy_reproduce = np.zeros((timesteps, self.n_dmps)) ddy_reproduce = np.zeros((timesteps, self.n_dmps)) for t in range(timesteps): y_reproduce[t], dy_reproduce[t], ddy_reproduce[t] = self.step( tau=tau) return y_reproduce, dy_reproduce, ddy_reproduce def step(self, tau=None): # run canonical system if tau == None: tau = self.tau x = self.cs.step_discrete(tau) # generate basis function activation psi = self.generate_psi(x) for d in range(self.n_dmps): # generate forcing term # ------------ Original DMP in Schaal 2002 # f = np.dot(psi, self.w[d])*x*(self.goal[d] - self.y0[d]) / np.sum(psi) # ---------- 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 # generate reproduced trajectory self.ddy[d] = self.alpha_y[d] * (self.beta_y[d] * (self.goal[d] - self.y[d]) - self.dy[d]) + f self.dy[d] += tau * self.ddy[d] * self.dt self.y[d] += tau * self.dy[d] * self.dt return self.y, self.dy, self.ddy
class dmp_rhythmic(): def __init__(self, n_dmps=1, n_bfs=100, dt=0, alpha_y=None, beta_y=None, **kwargs): self.n_dmps = n_dmps # number of data dimensions, one dmp for one degree self.n_bfs = n_bfs # number of basis functions self.dt = dt self.goal = np.ones(n_dmps) # for multiple dimensions self.alpha_y = np.ones(n_dmps) * 60.0 if alpha_y is None else alpha_y self.beta_y = self.alpha_y / 4.0 if beta_y is None else beta_y self.tau = 1.0 self.w = np.zeros((n_dmps, n_bfs)) # weights for forcing term self.psi_centers = np.zeros( self.n_bfs ) # centers over canonical system for Gaussian basis functions self.psi_h = np.zeros( self.n_bfs ) # variance over canonical system for Gaussian basis functions # canonical system self.cs = CanonicalSystem(dt=self.dt, type="rhythmic") self.timesteps = round(self.cs.run_time / self.dt) # generate centers for Gaussian basis functions self.generate_centers() self.h = np.ones(self.n_bfs) * self.n_bfs # reset state self.reset_state() # Reset the system state def reset_state(self): self.y = np.zeros(self.n_dmps) self.dy = np.zeros(self.n_dmps) self.ddy = np.zeros(self.n_dmps) self.cs.reset_state() def generate_centers(self): self.psi_centers = np.linspace(0, 2 * np.pi, self.n_bfs) return self.psi_centers def generate_psi(self, x): if isinstance(x, np.ndarray): x = x[:, None] self.psi = np.exp(self.h * (np.cos(x - self.psi_centers) - 1)) return self.psi def generate_weights(self, f_target): x_track = self.cs.run() psi_track = self.generate_psi(x_track) for d in range(self.n_dmps): for b in range(self.n_bfs): # note that here r is the default value, r == 1 numer = 1 * np.sum(psi_track[:, b] * f_target[:, d]) denom = np.sum(psi_track[:, b] + 1e-10) self.w[d, b] = numer / denom return self.w def learning(self, y_demo, plot=False): if y_demo.ndim == 1: # data is with only one dimension y_demo = y_demo.reshape(1, len(y_demo)) # For rhythmic DMPs, the goal is the average of the desired trajectory goal = np.zeros(self.n_dmps) for n in range(self.n_dmps): num_idx = ~np.isnan( y_demo[n]) # ignore nan's when calculating goal goal[n] = 0.5 * (y_demo[n, num_idx].min() + y_demo[n, num_idx].max()) self.goal = goal # interpolate the demonstrated trajectory to be the same length with timesteps x = np.linspace(0, self.cs.run_time, y_demo.shape[1]) y = np.zeros((self.n_dmps, self.timesteps)) for d in range(self.n_dmps): y_tmp = interp1d(x, y_demo[d]) for t in range(self.timesteps): y[d, t] = y_tmp(t * self.dt) # calculate velocity and acceleration of y_demo dy_demo = np.gradient(y, axis=1) / self.dt ddy_demo = np.gradient(dy_demo, axis=1) / self.dt f_target = np.zeros((y_demo.shape[1], self.n_dmps)) for d in range(self.n_dmps): f_target[:, d] = ddy_demo[d] - ( self.alpha_y[d] * (self.beta_y[d] * (self.goal[d] - y_demo[d]) - dy_demo[d])) self.generate_weights(f_target) if plot is True: # plot the basis function activations plt.figure() plt.subplot(211) psi_track = self.generate_psi(self.cs.run()) plt.plot(psi_track) plt.title('basis functions') # plot the desired forcing function vs approx plt.subplot(212) plt.plot(f_target[:, 0]) plt.plot(np.sum(psi_track * self.w[0], axis=1) * self.dt) plt.legend(['f_target', 'w*psi']) plt.title('DMP forcing function') plt.tight_layout() plt.show() # reset state self.reset_state() def reproduce(self, tau=None, goal=None, r=None): # set temporal scaling if tau == None: timesteps = self.timesteps else: timesteps = round(self.timesteps / tau) # set goal state if goal != None: self.goal = goal # set r if r == None: r = np.ones(self.n_dmps) # reset state self.reset_state() y_reproduce = np.zeros((timesteps, self.n_dmps)) dy_reproduce = np.zeros((timesteps, self.n_dmps)) ddy_reproduce = np.zeros((timesteps, self.n_dmps)) for t in range(timesteps): y_reproduce[t], dy_reproduce[t], ddy_reproduce[t] = self.step( tau=tau, r=r) return y_reproduce, dy_reproduce, ddy_reproduce def step(self, tau=None, r=None): # run canonical system if tau == None: tau = self.tau x = self.cs.step_rhythmic(tau) # generate basis function activation psi = self.generate_psi(x) for d in range(self.n_dmps): # generate forcing term f = r[d] * np.dot(psi, self.w[d]) / np.sum(psi) # generate reproduced trajectory self.ddy[d] = self.alpha_y[d] * (self.beta_y[d] * (self.goal[d] - self.y[d]) - self.dy[d]) + f self.dy[d] += tau * self.ddy[d] * self.dt self.y[d] += tau * self.dy[d] * self.dt return self.y, self.dy, self.ddy