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
