class DMPs(object): """Implementation of Dynamic Motor Primitives, as described in Dr. Stefan Schaal's (2002) paper.""" def __init__(self, dmps, bfs, dt=.01, y0=0, goal=1, w=None, ay=None, by=None, **kwargs): # CS parameters """ dmps int: number of dynamic motor primitives bfs int: number of basis functions per DMP dt float: timestep for simulation y0 list: initial state of DMPs goal list: goal state of DMPs w list: tunable parameters, control amplitude of basis functions ay int: gain on attractor term y dynamics by int: gain on attractor term y dynamics """ self.dmps = dmps self.bfs = bfs self.dt = dt if isinstance(y0, (int, float)): y0 = np.ones(self.dmps)*y0 self.y0 = y0 if isinstance(goal, (int, float)): goal = np.ones(self.dmps)*goal self.goal = goal if w is None: # default is f = 0 w = np.zeros((self.dmps, self.bfs)) self.w = w if ay is None: ay = np.ones(dmps)*25 # Schaal 2012 self.ay = ay if by is None: by = self.ay.copy() / 4 # Schaal 2012 self.by = by # set up the CS self.cs = CanonicalSystem(**kwargs) def gen_psi_track(self): raise NotImplementedError() def imitate_paths(self): raise NotImplementedError() def open_rollout(self): """Generate a system trial. Canonical system is run offline and no feedback is incorporated. """ timesteps = int(self.run_time / self.dt) # run canonical system, record activations x_track = self.cs.discrete_rollout(dt=self.dt, run_time=self.run_time) psi_track = self.gen_psi_track(x_track=x_track) # set system state y = self.y0.copy() dy = np.zeros(self.dmps) ddy = np.zeros(self.dmps) # set up tracking vectors y_track = np.zeros((timesteps, self.dmps)) # desired path dy_track = np.zeros((timesteps, self.dmps)) # desired velocity ddy_track = np.zeros((timesteps, self.dmps)) # desired acceleration for t in range(timesteps): for d in range(self.dmps): # generate the forcing term f = x_track[t] * (self.goal[d] - self.y0[d]) * \ (np.dot(psi_track[t], self.w[d])) / np.sum(psi_track[t]) # DMP acceleration ddy[d] = self.tau * (self.ay[d] * (self.by[d] * (self.goal[d] - y[d]) - dy[d]) + f) dy[d] += self.tau * ddy[d] * self.dt y[d] += self.tau * dy[d] * self.dt # record timestep y_track[t] = y dy_track[t] = dy ddy_track[t] = ddy return y_track, dy_track, ddy_track