def __init__(self,
n_dmps,
n_bfs,
dt=0.001,
run_time=6,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
self.run_time = run_time
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps) * y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps) * goal
self.goal = goal
if w is None:
# default is f = 0 so all the weights are null
w = np.zeros((self.n_dmps, self.n_bfs))
self.w = w
self.ay = np.ones(
n_dmps) * 25.0 if ay is None else ay # Schaal 2012 (*25)
self.by = self.ay / 4.0 if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, run_time=self.run_time, **kwargs)
self.timesteps = self.cs.timesteps
# set up the DMP system
self.reset_state(
) # this is a different reset with respect to CS (it's defined below)
self.gen_centers()
# set variance of Gaussian basis functions
self.h = np.ones(
self.n_bfs) * self.n_bfs**1.5 / self.c**1.8 / self.cs.ax
self.check_offset()
def __init__(self, dims, bfs, ts=None, dt=.01,
y0=0, goal=1, w=None,
ay=None, by=None, **kwargs):
'''
dims 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 = dims
self.bfs = bfs
self.dt = dt
# set up the Transformation System
if ts is None:
ts = TdwFormulation()
self.ts = ts
# start and goal
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
self.f_desired = np.array([])
self.f_predicted = np.array([])
self.f = np.zeros(self.dmps)
# set up the CS
self.cs = CanonicalSystem(pattern='discrete', dt=self.dt, **kwargs)
self.time_steps = int(self.cs.run_time / self.dt)
# set up the DMP system
self.reset_state()
self.prep_centers_and_variances()
self.check_offset()
def __init__(self,
n_dmps,
n_bfs,
dt=0.001,
run_time=6,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
self.run_time = run_time
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps) * y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps) * goal
self.goal = goal
if w is None:
# default is f = 0 so all the weights are null
w = np.zeros((self.n_dmps, self.n_bfs))
self.w = w
self.ay = np.ones(
n_dmps) * 25.0 if ay is None else ay # Schaal 2012 (*25)
self.by = self.ay / 4.0 if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, run_time=self.run_time, **kwargs)
self.timesteps = self.cs.timesteps
# set up the DMP system
self.reset_state()
self.gen_centers()
# HERE WE CHOOSE TO ADD **1.5 AT THE CENTERS "self.c"
self.h = np.ones(
self.n_bfs) * self.n_bfs**1.5 / self.c**1.8 / self.cs.ax
self.check_offset()
def __init__(self,
n_dmps,
n_bfs,
dt=.01,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
'''
:param n_dmps: int, number of dynamic motor primitives
:param n_bfs: int, number of basis function per DMP
:param dt: float, timestep of simulation
:param y0: list, initial state of DMPs
:param goal: list, goal state of DMPs
:param w: list, control amplitude of basis functions
:param ay: int, gain on attractor term y dynamic
:param by: int, gain on attractor term y dynamic
:param kwargs: param for dict data
'''
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
if isinstance(y0, (int, float)):
y0 = np.ones(n_dmps) * y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(n_dmps) * goal
self.goal = goal
if w is None:
# default f = 0
w = np.zeros((n_dmps, n_bfs))
self.w = w
self.ay = np.ones(n_dmps) * 25. if ay is None else ay
self.by = self.ay / 4. if by is None else by
# set up the cs
self.cs = CanonicalSystem(self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
# set up the DMP system
self.reset_state()
def __init__(self,
dmps,
bfs,
dt=.01,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
"""
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
# check if y0 is int or float type
if isinstance(y0, (int, float)):
y0 = np.ones(self.dmps) * y0
self.y0 = y0
# check if goal is int or float type
if isinstance(goal, (int, float)):
goal = np.ones(self.dmps) * goal
self.goal = goal
# w is a array dmpsxbfs vector/matrix
if w is None:
# default is f = 0
w = np.zeros((self.dmps, self.bfs))
self.w = w
self.ay = np.ones(dmps) * 25. if ay is None else ay # Schaal 2012
self.by = self.ay / 4. if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
# set up the DMP system
self.reset_state()
def __init__(self,
n_dmps,
n_bfs,
decay,
dt=.01,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
"""
n_dmps int: number of dynamic motor primitives
n_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.decay = decay
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps) * y0
self.y0 = np.array(y0)
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps) * goal
self.goal = np.array(goal)
if w is None:
# default is f = 0
w = np.zeros((self.n_dmps, self.n_bfs))
self.w = w
self.ay = np.ones(n_dmps) * 25. if ay is None else ay # Schaal 2012
self.by = self.ay / 4. if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
self.gen_centers()
self.h = np.ones(self.n_bfs) * self.n_bfs**1.5 / self.c / self.cs.ax
# set up the DMP system
self.reset_state()
def __init__(self,
nDMPs,
nBFs,
dt=0.01,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
"""
Default values from Schaal (2012)
[INPUT]
[VAR NAME] [TYPE] [DESCRIPTION]
(1) nDMPs int Number of dynamic motor primitives
(2) nBFs int Number of basis functions per DMP
(3) dt float Timestep for simulation
(4) y0 list Initial state of DMPs
(5) goal list Goal state of DMPs
(6) w list Tunable parameters, control amplitude of basis functions
(7) ay int Gain on attractor term y dynamics
(8) by int Gain on attractor term y dynamics
"""
self.nDMPs = nDMPs
self.nBFs = nBFs
self.dt = dt
self.y0 = y0 * np.ones(self.nDMPs) if isinstance(y0,
(int, float)) else y0
self.goal = goal * np.ones(self.nDMPs) if isinstance(
goal, (int, float)) else goal
self.w = np.zeros((self.nDMPs, self.nBFs)) if w is None else w
self.ay = np.ones(nDMPs) * 25.0 if ay is None else ay # Schaal 2012
self.by = self.ay / 4.0 if by is None else by # Schaal 2012, 0.25 makes it a critically damped system
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.runTime / self.dt)
# set up the DMP system
self.reset_state()
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()
def __init__(self, ID, dmps, bfs, dt=.01,
y0=0, goal=1, w=None,
ay=None, by=None, **kwargs):
"""
ID int / string: identifies this DMP group
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.ID = ID
self.dmps = dmps
self.bfs = bfs
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(dt,**kwargs)
self.timesteps = int(self.cs.run_time / dt)
# set up the DMP system
self.reset_state()
class DMPs(object):
def __init__(self,
n_dmps,
n_bfs,
dt=0.001,
run_time=6,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
self.run_time = run_time
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps) * y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps) * goal
self.goal = goal
if w is None:
# default is f = 0 so all the weights are null
w = np.zeros((self.n_dmps, self.n_bfs))
self.w = w
self.ay = np.ones(
n_dmps) * 25.0 if ay is None else ay # Schaal 2012 (*25)
self.by = self.ay / 4.0 if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, run_time=self.run_time, **kwargs)
self.timesteps = self.cs.timesteps
# set up the DMP system
self.reset_state()
self.gen_centers()
# HERE WE CHOOSE TO ADD **1.5 AT THE CENTERS "self.c"
self.h = np.ones(
self.n_bfs) * self.n_bfs**1.5 / self.c**1.8 / self.cs.ax
self.check_offset()
def gen_centers(self):
# desired activations throughout time
des_c = np.linspace(0, self.cs.run_time, self.n_bfs)
self.c = np.ones(len(des_c))
for n in range(len(des_c)):
# finding x for desired times t
self.c[n] = np.exp(-self.cs.ax * des_c[n]) #it should be /tau
def gen_psi(self, x):
if isinstance(x, np.ndarray):
x = x[:, None]
return np.exp(-self.h * (x - self.c)**2)
def gen_weights(self, f_target):
# calculate x and psi
x_track = self.cs.rollout()
psi_track = self.gen_psi(x_track)
# efficiently calculate BF weights using weighted linear regression
self.w = np.zeros((self.n_dmps, self.n_bfs))
for d in range(self.n_dmps):
# spatial scaling term
k = self.goal[d] - self.y0[d]
for b in range(self.n_bfs):
numer = np.sum(x_track * psi_track[:, b] * f_target[:, d])
denom = np.sum(x_track**2 * psi_track[:, b])
self.w[d, b] = numer / denom
# if abs(k) > 1e-5: THIS HAS BEEN REMOVED
self.w[d, b] /= k
self.w = np.nan_to_num(self.w)
def imitate_path(self, y_des, plot=False):
# set initial state and goal
if y_des.ndim == 1:
y_des = y_des.reshape(1, len(y_des)) # to be sure it's a row
self.y0 = y_des[:,
0].copy() # dynamic link between a vector and its copy
self.y_des = y_des.copy()
self.goal = np.copy(y_des[:, -1])
# self.check_offset()
# generate function to interpolate the desired trajectory
import scipy.interpolate
path = np.zeros((self.n_dmps, self.timesteps))
x = np.linspace(0, self.cs.run_time,
y_des.shape[1]) #number of columns of Y
for d in range(self.n_dmps):
path_gen = scipy.interpolate.interp1d(
x, y_des[d]) #path_gen is a function (not an array)
for t in range(self.timesteps):
path[d, t] = path_gen(t * self.dt)
y_des = path #this changes also the other linked array with function "copy"
# calculate velocity of y_des with central differences
dy_des = np.gradient(y_des, axis=1) / self.dt
# calculate acceleration of y_des with central differences
ddy_des = np.gradient(dy_des, axis=1) / self.dt
f_target = np.zeros(
(y_des.shape[1],
self.n_dmps)) #row: columns of y_des. col = n_dmps
# find the force required to move along this trajectory
for d in range(self.n_dmps):
f_target[:, d] = ddy_des[d] - self.ay[d] * (
self.by[d] * (self.goal[d] - y_des[d]) - dy_des[d])
# efficiently generate weights to realize f_target
self.gen_weights(f_target)
if plot is True:
# plot the basis function activations
import matplotlib.pyplot as plt
plt.figure()
plt.subplot(211)
psi_track = self.gen_psi(self.cs.rollout())
plt.plot(psi_track)
plt.title("basis functions")
# plot the desired forcing function vs approx
for ii in range(self.n_dmps):
plt.subplot(2, self.n_dmps, self.n_dmps + 1 + ii)
plt.plot(f_target[:, ii],
"o--",
label="computed f_target of DoF %i" % ii)
for ii in range(self.n_dmps):
plt.subplot(2, self.n_dmps, self.n_dmps + 1 + ii)
print("w shape: ", self.w.shape)
plt.plot(np.sum(psi_track * self.w[ii], axis=1) * self.dt,
"o-",
label="sum(weight * psi %i)" % ii)
plt.legend()
plt.title("DMP forcing function")
plt.tight_layout()
plt.get_current_fig_manager().window.showMaximized()
plt.show()
self.reset_state()
return y_des
def rollout(self, timesteps=None, v_vect=None, **kwargs):
self.reset_state()
if timesteps is None:
if "tau" in kwargs:
timesteps = int(self.timesteps / kwargs["tau"])
else:
timesteps = self.timesteps
# set up tracking vectors
y_track = np.zeros((timesteps, self.n_dmps))
dy_track = np.zeros((timesteps, self.n_dmps))
ddy_track = np.zeros((timesteps, self.n_dmps))
for t in range(timesteps):
# run and record timestep
y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs)
return y_track, dy_track, ddy_track
def step(self, tau=1.0, error=0.0, external_force=None):
error_coupling = 1.0 / (1.0 + error)
# run canonical system
x = self.cs.step(tau=tau, error_coupling=error_coupling)
# generate basis function activation
psi = self.gen_psi(x)
for d in range(self.n_dmps):
# generate the forcing term
f = x * (self.goal[d] - self.y0[d]) * (np.dot(
psi, self.w[d])) / np.sum(psi)
# DMP acceleration
self.ddy[d] = (self.ay[d] *
(self.by[d] *
(self.goal[d] - self.y[d]) - self.dy[d]) + f)
if external_force is not None:
self.ddy[d] += external_force[d]
self.dy[d] += self.ddy[d] * tau * self.dt * error_coupling
self.y[d] += self.dy[d] * tau * self.dt * error_coupling
return self.y, self.dy, self.ddy
def check_offset(self):
for d in range(self.n_dmps):
if abs(self.y0[d] - self.goal[d]) < 1e-2:
self.goal[d] += 1e-2
def reset_state(self):
self.y = self.y0.copy(
) #this fix the value of y (if y changes, y0 doesn't change)
self.dy = np.zeros(self.n_dmps)
self.ddy = np.zeros(self.n_dmps)
self.cs.reset_state()
class DMPs_discrete(object):
def __init__(self, dims, bfs, ts=None, dt=.01,
y0=0, goal=1, w=None,
ay=None, by=None, **kwargs):
'''
dims 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 = dims
self.bfs = bfs
self.dt = dt
# set up the Transformation System
if ts is None:
ts = TdwFormulation()
self.ts = ts
# start and goal
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
self.f_desired = np.array([])
self.f_predicted = np.array([])
self.f = np.zeros(self.dmps)
# set up the CS
self.cs = CanonicalSystem(pattern='discrete', dt=self.dt, **kwargs)
self.time_steps = int(self.cs.run_time / self.dt)
# set up the DMP system
self.reset_state()
self.prep_centers_and_variances()
self.check_offset()
def prep_centers_and_variances(self):
'''
Set the centre of the Gaussian basis functions be spaced evenly
throughout run time.
'''
# desired spacings along x
# need to be spaced evenly between 1 and exp(-ax)
# lowest number should be only as far as x gets
first = np.exp(-self.cs.ax * self.cs.run_time)
last = 1.05 - first
des_c = np.linspace(first, last, self.bfs)
self.c = np.ones(len(des_c))
for n in range(len(des_c)):
self.c[n] = -np.log(des_c[n])
# set variance of Gaussian basis functions
# trial and error to find this spacing
self.h = np.ones(self.bfs) * self.bfs**1.5 / self.c
def check_offset(self):
'''
Check to see if initial position and goal are the same
if they are, offset slightly so that the forcing term is not 0.
'''
for idx in range(self.dmps):
if (self.y0[idx] == self.goal[idx]):
self.goal[idx] += 1e-4
def set_goal(self, y_des):
'''
Generate the goal for path imitation. For rhythmic DMPs the goal is the
average of the desired trajectory.
y_des np.array: the desired trajectory to follow
'''
return y_des[:,-1].copy()
def gen_psi(self, x):
'''
Generates the activity of the basis functions for a given state of the
canonical system.
x float: the current state of the canonical system
'''
if isinstance(x, np.ndarray):
x = x[:,None]
return np.exp(-self.h * (x - self.c)**2)
def gen_forcing_term(self, x, dmp_num, scale=False):
"""Calculates the complete forcing term .
x float: the current value of the canonical system (s)
dmp_num int: the index of the current dmp
scaled bool: apply scalation (g-y0) to the forcing term (T/F)
"""
psi = self.gen_psi(x)
f = x * ((np.dot(psi, self.w[dmp_num])) / np.sum(psi))
if scale:
f = f * (self.goal[dmp_num] - self.y0[dmp_num])
return f
def find_force_function(self, dmp_num, y, dy, ddy, s):
d = dmp_num
f_target = self.ts.fs(y[d], dy[d], ddy[d], self.y0[d], self.goal[d], 1.0, s)
return f_target
def calculate_acceleration(self, dmp_num, tau, f, s):
d = dmp_num
ddy = self.ts.acceleration(self.y[d], self.dy[d], self.y0[d], self.goal[d], tau, f, s)
return ddy
def compute_weights(self, f_target):
'''
Generate a set of weights over the basis functions such that the target forcing
term trajectory is matched.
f_target np.array: the desired forcing term
'''
# calculate x/s and psi(s)
x_track = self.cs.rollout()
psi_track = self.gen_psi(x_track)
# efficiently calculate weights for BFs using weighted linear regression
self.w = np.zeros((self.dmps, self.bfs))
for d in range(self.dmps):
for b in range(self.bfs):
numer = np.sum(x_track * psi_track[:,b] * f_target[:,d])
denom = np.sum(x_track**2 * psi_track[:,b])
self.w[d,b] = numer / denom
return (x_track, psi_track)
def reset_state(self):
'''
Reset the system state
'''
self.y = self.y0.copy()
self.dy = np.zeros(self.dmps)
self.ddy = np.zeros(self.dmps)
self.cs.reset_state()
def step(self, tau=1.0, state_fb=None, external_force=None):
'''
Run the DMP system for a single timestep.
tau float: scales the timestep
increase tau to make the system execute faster
state_fb np.array: optional system feedback
'''
# run canonical system
cs_args = {'tau': tau, 'error_coupling': 1.0}
if state_fb is not None:
# take the 2 norm of the overall error
state_fb = state_fb.reshape(1,self.dmps)
dist = np.sqrt(np.sum((state_fb - self.y)**2))
cs_args['error_coupling'] = 1.0 / (1.0 + 10*dist)
x = self.cs.step(**cs_args)
for idx in range(self.dmps):
# Calcualte acceleration based on f(s)
self.f[idx] = self.gen_forcing_term(x, idx)
self.ddy[idx] = self.calculate_acceleration(idx, tau, self.f[idx], x)
# Correct acceleration
if external_force is not None:
self.ddy[idx] += external_force[idx]
self.dy[idx] += self.ddy[idx] * tau * self.dt * cs_args['error_coupling']
self.y[idx] += self.dy[idx] * self.dt * cs_args['error_coupling']
return self.y, self.dy, self.ddy
def learn(self, y_des):
'''
Takes in a desired trajectory and generates the set of system parameters
that best realize this path.
y_des list/array: the desired trajectories of each DMP
should be shaped [dmps, run_time]
'''
# set initial state and goal
# ---
if y_des.ndim == 1:
y_des = y_des.reshape(1,len(y_des))
self.y0 = y_des[:,0].copy()
self.y_des = y_des.copy()
self.goal = self.set_goal(y_des)
self.check_offset()
# generate function to interpolate the desired trajectory
# ---
import scipy.interpolate
path = np.zeros((self.dmps, self.time_steps))
x = np.linspace(0, self.cs.run_time, y_des.shape[1])
for idx in range(self.dmps):
path_gen = scipy.interpolate.interp1d(x, y_des[idx])
for t in range(self.time_steps):
path[idx, t] = path_gen(t * self.dt)
y_des = path
# Calculate velocity and acceleration profiles
# ---
# calculate velocity of y_des
dy_des = np.diff(y_des) / self.dt
# add zero to the beginning of every row
dy_des = np.hstack((np.zeros((self.dmps, 1)), dy_des))
# calculate acceleration of y_des
ddy_des = np.diff(dy_des) / self.dt
# add zero to the beginning of every row
ddy_des = np.hstack((np.zeros((self.dmps, 1)), ddy_des))
# Compute F and weights
# ---
# run canonical system
x = self.cs.rollout()
# find the force required to move along this trajectory
f_desired = np.zeros((y_des.shape[1], self.dmps))
for idx in range(self.dmps):
f_desired[:,idx] = self.find_force_function(idx, y_des, dy_des, ddy_des, x)
# efficiently generate weights to realize f_target
self.x_track, self.psi_track = self.compute_weights(f_desired)
self.f_desired = f_desired
self.reset_state()
return y_des, dy_des, ddy_des
def plan(self, time_steps=None, **kwargs):
'''
Generate a system trial, no feedback is incorporated.
'''
self.reset_state()
if time_steps is None:
if kwargs.has_key('tau'):
time_steps = int(self.time_steps / kwargs['tau'])
else:
time_steps = self.time_steps
# set up tracking vectors
y_track = np.zeros((time_steps, self.dmps))
dy_track = np.zeros((time_steps, self.dmps))
ddy_track = np.zeros((time_steps, self.dmps))
f_predicted = np.zeros((time_steps, self.dmps))
for t in range(time_steps):
y, dy, ddy = self.step(**kwargs)
f_predicted[t] = self.f
# record timestep
y_track[t] = y
dy_track[t] = dy
ddy_track[t] = ddy
self.f_predicted = f_predicted
return y_track, dy_track, ddy_track
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):
"""
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
self.ay = np.ones(dmps) * 25. if ay is None else ay # Schaal 2012
self.by = self.ay / 4. if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
# set up the DMP system
self.reset_state()
def check_offset(self):
"""Check to see if initial position and goal are the same
if they are, offset slightly so that the forcing term is not 0"""
for d in range(self.dmps):
if (self.y0[d] == self.goal[d]):
self.goal[d] += 1e-4
def gen_front_term(self, x, dmp_num):
raise NotImplementedError()
def gen_goal(self, y_des):
raise NotImplementedError()
def gen_psi(self):
raise NotImplementedError()
def gen_weights(self, f_target):
raise NotImplementedError()
def imitate_path(self, y_des):
"""Takes in a desired trajectory and generates the set of
system parameters that best realize this path.
y_des list/array: the desired trajectories of each DMP
should be shaped [dmps, run_time]
"""
# set initial state and goal
if y_des.ndim == 1:
y_des = y_des.reshape(1, len(y_des))
self.y0 = y_des[:, 0].copy()
self.y_des = y_des.copy()
self.goal = self.gen_goal(y_des)
self.check_offset()
# generate function to interpolate the desired trajectory
import scipy.interpolate
path = np.zeros((self.dmps, self.timesteps))
x = np.linspace(0, self.cs.run_time, y_des.shape[1])
for d in range(self.dmps):
path_gen = scipy.interpolate.interp1d(x, y_des[d])
for t in range(self.timesteps):
path[d, t] = path_gen(t * self.dt)
y_des = path
# calculate velocity of y_des
dy_des = np.diff(y_des) / self.dt
# add zero to the beginning of every row
dy_des = np.hstack((np.zeros((self.dmps, 1)), dy_des))
# calculate acceleration of y_des
ddy_des = np.diff(dy_des) / self.dt
# add zero to the beginning of every row
ddy_des = np.hstack((np.zeros((self.dmps, 1)), ddy_des))
f_target = np.zeros((y_des.shape[1], self.dmps))
# find the force required to move along this trajectory
for d in range(self.dmps):
f_target[:, d] = (ddy_des[d] - self.ay[d] *
(self.by[d] *
(self.goal[d] - y_des[d]) - dy_des[d]))
# efficiently generate weights to realize f_target
self.gen_weights(f_target)
'''# plot the basis function activations
import matplotlib.pyplot as plt
plt.figure()
plt.subplot(211)
plt.plot(psi_track)
plt.title('psi_track')
# 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))
plt.legend(['f_target', 'w*psi'])
plt.tight_layout()
plt.show()'''
self.reset_state()
return y_des
def rollout(self, timesteps=None, **kwargs):
"""Generate a system trial, no feedback is incorporated."""
self.reset_state()
if timesteps is None:
if 'tau' in kwargs:
timesteps = int(self.timesteps / kwargs['tau'])
else:
timesteps = self.timesteps
# set up tracking vectors
y_track = np.zeros((timesteps, self.dmps))
dy_track = np.zeros((timesteps, self.dmps))
ddy_track = np.zeros((timesteps, self.dmps))
for t in range(timesteps):
y, dy, ddy = self.step(**kwargs)
# record timestep
y_track[t] = y
dy_track[t] = dy
ddy_track[t] = ddy
return y_track, dy_track, ddy_track
def reset_state(self):
"""Reset the system state"""
self.y = self.y0.copy()
self.dy = np.zeros(self.dmps)
self.ddy = np.zeros(self.dmps)
self.cs.reset_state()
def step(self, tau=1.0, error=0.0, external_force=None):
"""Run the DMP system for a single timestep.
tau float: scales the timestep
increase tau to make the system execute faster
error float: optional system feedback
"""
error_coupling = 1.0 / (1.0 + error)
# run canonical system
x = self.cs.step(tau=tau, error_coupling=error_coupling)
# generate basis function activation
psi = self.gen_psi(x)
for d in range(self.dmps):
# generate the forcing term
f = (self.gen_front_term(x, d) * (np.dot(psi, self.w[d])) /
np.sum(psi))
# DMP acceleration
self.ddy[d] = (self.ay[d] *
(self.by[d] *
(self.goal[d] - self.y[d]) - self.dy[d] / tau) +
f) * tau
if external_force is not None:
self.ddy[d] += external_force[d]
self.dy[d] += self.ddy[d] * tau * self.dt * error_coupling
self.y[d] += self.dy[d] * self.dt * error_coupling
return self.y, self.dy, self.ddy
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):
"""
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(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
# set up the DMP system
self.reset_state()
def check_offset(self):
"""Check to see if initial position and goal are the same
if they are, offset slightly so that the forcing term is not 0"""
for d in range(self.dmps):
if (self.y0[d] == self.goal[d]):
self.goal[d] += 1e-4
def gen_front_term(self, x, dmp_num): raise NotImplementedError()
def gen_goal(self, y_des): raise NotImplementedError()
def gen_psi(self): raise NotImplementedError()
def gen_weights(self, f_target): raise NotImplementedError()
def imitate_path(self, y_des):
"""Takes in a desired trajectory and generates the set of
system parameters that best realize this path.
y_des list/array: the desired trajectories of each DMP
should be shaped [dmps, run_time]
"""
# set initial state and goal
if y_des.ndim == 1:
y_des = y_des.reshape(1,len(y_des))
self.y0 = y_des[:,0].copy()
self.y_des = y_des.copy()
self.goal = self.gen_goal(y_des)
self.check_offset()
# generate function to interpolate the desired trajectory
import scipy.interpolate
path = np.zeros((self.dmps, self.timesteps))
x = np.linspace(0, self.cs.run_time, y_des.shape[1])
for d in range(self.dmps):
path_gen = scipy.interpolate.interp1d(x, y_des[d])
for t in range(self.timesteps):
path[d, t] = path_gen(t * self.dt)
y_des = path
# calculate velocity of y_des
dy_des = np.diff(y_des) / self.dt
# add zero to the beginning of every row
dy_des = np.hstack((np.zeros((self.dmps, 1)), dy_des))
# calculate acceleration of y_des
ddy_des = np.diff(dy_des) / self.dt
# add zero to the beginning of every row
ddy_des = np.hstack((np.zeros((self.dmps, 1)), ddy_des))
f_target = np.zeros((y_des.shape[1], self.dmps))
# find the force required to move along this trajectory
for d in range(self.dmps):
f_target[:,d] = ddy_des[d] - self.ay[d] * \
(self.by[d] * (self.goal[d] - y_des[d]) - \
dy_des[d])
# efficiently generate weights to realize f_target
self.gen_weights(f_target)
'''# plot the basis function activations
import matplotlib.pyplot as plt
plt.figure()
plt.subplot(211)
plt.plot(psi_track)
plt.title('psi_track')
# 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))
plt.legend(['f_target', 'w*psi'])
plt.tight_layout()
plt.show()'''
self.reset_state()
return y_des
def rollout(self, timesteps=None, **kwargs):
"""Generate a system trial, no feedback is incorporated."""
self.reset_state()
if timesteps is None:
if kwargs.has_key('tau'):
timesteps = int(self.timesteps / kwargs['tau'])
else:
timesteps = self.timesteps
# set up tracking vectors
y_track = np.zeros((timesteps, self.dmps))
dy_track = np.zeros((timesteps, self.dmps))
ddy_track = np.zeros((timesteps, self.dmps))
for t in range(timesteps):
y, dy, ddy = self.step(**kwargs)
# record timestep
y_track[t] = y
dy_track[t] = dy
ddy_track[t] = ddy
return y_track, dy_track, ddy_track
def reset_state(self):
"""Reset the system state"""
self.y = self.y0.copy()
self.dy = np.zeros(self.dmps)
self.ddy = np.zeros(self.dmps)
self.cs.reset_state()
def step(self, tau=1.0, state_fb=None):
"""Run the DMP system for a single timestep.
tau float: scales the timestep
increase tau to make the system execute faster
state_fb np.array: optional system feedback
"""
# run canonical system
cs_args = {'tau':tau,
'error_coupling':1.0}
if state_fb is not None:
# take the 2 norm of the overall error
state_fb = state_fb.reshape(1,self.dmps)
dist = np.sqrt(np.sum((state_fb - self.y)**2))
cs_args['error_coupling'] = 1.0 / (1.0 + 10*dist)
x = self.cs.step(**cs_args)
# generate basis function activation
psi = self.gen_psi(x)
for d in range(self.dmps):
# generate the forcing term
f = self.gen_front_term(x, d) * \
(np.dot(psi, self.w[d])) / np.sum(psi)
# DMP acceleration
self.ddy[d] = (self.ay[d] *
(self.by[d] * (self.goal[d] - self.y[d]) - \
self.dy[d]/tau) + f) * tau**2
self.dy[d] += self.ddy[d] * self.dt * cs_args['error_coupling']
self.y[d] += self.dy[d] * self.dt * cs_args['error_coupling']
return self.y, self.dy, self.ddy
class DMPs(object):
def __init__(self,
n_dmps,
n_bfs,
dt=0.001,
run_time=6,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
self.run_time = run_time
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps) * y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps) * goal
self.goal = goal
if w is None:
# default is f = 0 so all the weights are null
w = np.zeros((self.n_dmps, self.n_bfs))
self.w = w
self.ay = np.ones(
n_dmps) * 25.0 if ay is None else ay # Schaal 2012 (*25)
self.by = self.ay / 4.0 if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, run_time=self.run_time, **kwargs)
self.timesteps = self.cs.timesteps
# set up the DMP system
self.reset_state(
) # this is a different reset with respect to CS (it's defined below)
self.gen_centers()
# set variance of Gaussian basis functions
self.h = np.ones(
self.n_bfs) * self.n_bfs**1.5 / self.c**1.8 / self.cs.ax
self.check_offset()
def gen_centers(self):
# desired activations throughout time
des_c = np.linspace(0, self.cs.run_time, self.n_bfs)
self.c = np.ones(len(des_c))
for n in range(len(des_c)):
# finding x for desired times t
self.c[n] = np.exp(-self.cs.ax * des_c[n]) #it should be /tau
def gen_psi(self, x):
if isinstance(x, np.ndarray):
x = x[:, None]
return np.exp(-self.h * (x - self.c)**2)
def gen_weights(self, f_target):
# calculate x and psi
x_track = self.cs.rollout()
psi_track = self.gen_psi(x_track)
# efficiently calculate BF weights using weighted linear regression
self.w = np.zeros((self.n_dmps, self.n_bfs))
for d in range(self.n_dmps):
# spatial scaling term
k = self.goal[d] - self.y0[d]
for b in range(self.n_bfs):
numer = np.sum(x_track * psi_track[:, b] * f_target[:, d])
denom = np.sum(x_track**2 * psi_track[:, b])
self.w[d, b] = numer / denom
# if abs(k) > 1e-5: THIS HAS BEEN REMOVED
self.w[d, b] /= k
self.w = np.nan_to_num(self.w)
def imitate_path(self, y_des, plot=False):
# set initial state and goal
if y_des.ndim == 1:
y_des = y_des.reshape(1, len(y_des)) # to be sure it's a row
self.y0 = y_des[:,
0].copy() # dynamic link between a vector and its copy
self.y_des = y_des.copy()
self.goal = np.copy(y_des[:, -1])
# self.check_offset()
# generate function to interpolate the desired trajectory
import scipy.interpolate
path = np.zeros((self.n_dmps, self.timesteps))
x = np.linspace(0, self.cs.run_time,
y_des.shape[1]) #number of columns of Y
for d in range(self.n_dmps):
path_gen = scipy.interpolate.interp1d(
x, y_des[d]) #path_gen is a function (not an array)
for t in range(self.timesteps):
path[d, t] = path_gen(t * self.dt)
y_des = path #this changes also the other linked array with function "copy"
# calculate velocity of y_des with central differences
dy_des = np.gradient(y_des, axis=1) / self.dt
# calculate acceleration of y_des with central differences
ddy_des = np.gradient(dy_des, axis=1) / self.dt
f_target = np.zeros(
(y_des.shape[1],
self.n_dmps)) #row: columns of y_des. col = n_dmps
# find the force required to move along this trajectory
for d in range(self.n_dmps):
f_target[:, d] = ddy_des[d] - self.ay[d] * (
self.by[d] * (self.goal[d] - y_des[d]) - dy_des[d])
# efficiently generate weights to realize f_target
self.gen_weights(f_target)
if plot is True:
# plot the basis function activations
import matplotlib.pyplot as plt
plt.figure()
plt.subplot(211)
psi_track = self.gen_psi(self.cs.rollout())
plt.plot(psi_track)
plt.title("basis functions")
# plot the desired forcing function vs approx
for ii in range(self.n_dmps):
plt.subplot(2, self.n_dmps, self.n_dmps + 1 + ii)
plt.plot(f_target[:, ii],
"o--",
label="computed f_target of DoF %i" % ii)
for ii in range(self.n_dmps):
plt.subplot(2, self.n_dmps, self.n_dmps + 1 + ii)
print("w shape: ", self.w.shape)
plt.plot(np.sum(psi_track * self.w[ii], axis=1) * self.dt,
"o-",
label="sum(weight * psi %i)" % ii)
plt.legend()
plt.title("DMP forcing function")
plt.tight_layout()
plt.get_current_fig_manager().window.showMaximized()
plt.show()
self.reset_state()
return y_des
def step(self, ya, error=0.0, tau=1.0):
self.ae = 5.0
kp = 35.0 # 35.0
kv = 8.0 # 10.0
kc = 0.5 # #(original value:10000)
tau = 1.0
tau_adapt = tau * (1 + (kc * error**2))
# run canonical system
# generate basis function activation
psi = self.gen_psi(self.cs.x)
for d in range(self.n_dmps):
self.dya[d] = (ya[d] - self.ya_old[d]) / self.dt
self.ya_old[d] = ya[d]
# Feed Forward Control on ACTUAL ROBOT MOVEMENT
self.ddyr[d] = kp * (self.yc[d] - ya[d]) + kv * (
self.dyc[d] - self.dya[d]) + self.ddyc[d]
# generate the forcing term
f = self.cs.x * (self.goal[d] - self.y0[d]) * (np.dot(
psi, self.w[d])) / np.sum(psi)
#f = np.nan_to_num(f)
# z coupling
self.dz[d] = 1 / tau_adapt * (
self.ay[d] * (self.by[d] *
(self.goal[d] - self.yc[d]) - self.z[d]) + f)
self.z[d] += self.dz[d] * self.dt
self.dyc[d] = self.z[
d] #/ tau_adapt # BISOGNA CAPIRE SE GIUSTO O NO
self.ddyc[d] = (self.dz[d] * tau_adapt -
tau * self.z[d] * 2 * kc * error *
(self.ae *
(ya[d] - self.yc[d] - error))) / tau_adapt**2
self.yc[d] += self.dyc[d] * self.dt
_ = self.cs.step(tau=tau_adapt)
return self.yc, self.dyc, self.ddyc, self.ddyr, tau_adapt
def check_offset(self):
for d in range(self.n_dmps):
if abs(self.y0[d] - self.goal[d]) < 1e-2:
self.goal[d] += 1e-2
def reset_state(self):
self.ya_old = self.y0.copy()
self.dya = np.zeros(self.n_dmps)
self.ddyr = np.zeros(self.n_dmps)
self.yc = self.y0.copy(
) #this fix the value of y (if y changes, y0 doesn't change)
self.dyc = np.zeros(self.n_dmps)
self.ddyc = np.zeros(self.n_dmps)
self.cs.reset_state()
self.dz = np.zeros(self.n_dmps)
self.z = np.zeros(self.n_dmps)
class DMPs(object):
def __init__(self,
n_dmps,
n_bfs,
dt=.01,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
'''
:param n_dmps: int, number of dynamic motor primitives
:param n_bfs: int, number of basis function per DMP
:param dt: float, timestep of simulation
:param y0: list, initial state of DMPs
:param goal: list, goal state of DMPs
:param w: list, control amplitude of basis functions
:param ay: int, gain on attractor term y dynamic
:param by: int, gain on attractor term y dynamic
:param kwargs: param for dict data
'''
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
if isinstance(y0, (int, float)):
y0 = np.ones(n_dmps) * y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(n_dmps) * goal
self.goal = goal
if w is None:
# default f = 0
w = np.zeros((n_dmps, n_bfs))
self.w = w
self.ay = np.ones(n_dmps) * 25. if ay is None else ay
self.by = self.ay / 4. if by is None else by
# set up the cs
self.cs = CanonicalSystem(self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
# set up the DMP system
self.reset_state()
def check_offset(self):
'''
Check to see if initial position and goal are the same
if they are, offset slightly so that the forcing term is not 0
'''
for d in range(self.n_dmps):
if (self.y0[d] == self.goal[d]):
self.goal[0] += 1e-4
def gen_front_term(self, x, dmp_num):
# Generate locally weight regression param
# return x * (self.goal[dmp_num] - self.y0[dmp_num])
raise NotImplementedError()
def gen_goal(self, y_des):
# Generate the goal for path imitation
# y_des: np.array, the desire trajectory to follow
raise NotImplementedError()
#return np.copy(y_des[:, -1])
def gen_psi(self, x):
# Generate the basis functions for a given canonical system rollout x
# x: float, array: the canonical system state x or path
raise NotImplementedError()
# if isinstance(x, np.ndarray):
# x = x[:, None]
# return np.exp(-self.h * (x - self.c)**2)
def gen_weights(self, f_target):
raise NotImplementedError()
def imitate_path(self, y_des, plot=False):
'''
Generate weight w
Takes in a desired trajectory and generate the set of system parameters that best realize this path
:param y_des: list/array: the desired trajectories of each DMP, shape [n_dmps, run_time]
:return: y_des
'''
# set initial state and goal, the desired goal
if y_des.ndim == 1:
y_des = y_des.reshape(1, len(y_des))
self.y0 = y_des[:, 0].copy()
self.y_des = y_des.copy()
self.goal = self.gen_goal(y_des)
self.check_offset()
# generate function to interpolate the desired trajectory
import scipy.interpolate
path = np.zeros((self.n_dmps, self.timesteps))
x = np.linspace(0, self.cs.run_time, y_des.shape[1])
for d in range(self.n_dmps):
path_gen = scipy.interpolate.interp1d(x, y_des[d])
for t in range(self.timesteps):
path[d, t] = path_gen(t * self.dt)
y_des = path
# calculate the velocity of y_des
dy_des = np.diff(y_des) / self.dt
# add zero to the beginning of every row
dy_des = np.hstack((np.zeros((self.n_dmps, 1)), dy_des))
# calculate the accelerate of y_des
ddy_des = np.diff(dy_des) / self.dt
# add zero to the beginning of every row
ddy_des = np.hstack((np.zeros((self.n_dmps, 1)), ddy_des))
# calculate f target function, shape [timesteps, n_dmps]
f_target = np.zeros((y_des.shape[1], self.n_dmps))
# find the force required to move along this trajectory
for d in range(self.n_dmps):
f_target[:, d] = (ddy_des[d] - self.ay[d] *
(self.by[d] *
(self.goal[d] - y_des[d]) - dy_des[d]))
# efficiency generate weight to realized f_target
self.gen_weights(f_target)
# if plot is True:
self.reset_state()
return y_des
def rollout(self, timesteps=None, **kwargs):
'''
Run step for timesteps to generate a system trial
:param timesteps:
:param kwargs:
:return: y_track, dy_track, ddy_track
'''
"""Generate a system trial, no feedback is incorporated."""
self.reset_state()
if timesteps is None:
if 'tau' in kwargs:
timesteps = int(self.timesteps / kwargs['tau'])
else:
timesteps = self.timesteps
# set up tracking vectors
y_track = np.zeros((timesteps, self.n_dmps))
dy_track = np.zeros((timesteps, self.n_dmps))
ddy_track = np.zeros((timesteps, self.n_dmps))
psi = np.zeros((timesteps, self.n_bfs))
for t in range(timesteps):
# run and record timestep
y_track[t], dy_track[t], ddy_track[t], psi[t] = self.step(**kwargs)
return y_track, dy_track, ddy_track, psi
def reset_state(self):
"""Reset the system state"""
self.y = self.y0.copy()
self.dy = np.zeros(self.n_dmps)
self.ddy = np.zeros(self.n_dmps)
self.cs.reset_state()
def step(self, tau=1.0, error=0.0, external_force=None):
'''
Run the DMP system for a single timestep.
:param tau: float, scales the timestep, increase tau to make the system execute faster
:param error: optional system feedback to adjust timestep
:return: self.y, self.dy, self.ddy
'''
error_coupling = 1.0 / (1.0 + error)
# run canonical system
x = self.cs.step(tau=tau, error_coupling=error_coupling)
# generate basis function activation
psi = self.gen_psi(x)
# print('psi',psi)
for d in range(self.n_dmps):
# generate forcing term
f = (self.gen_front_term(x, d) * (np.dot(psi, self.w[d])) /
np.sum(psi))
# DMP acceleration
# calculate y'' by f
self.ddy[d] = (self.ay[d] *
(self.by[d] *
(self.goal[d] - self.y[d]) - self.dy[d] / tau) +
f) * tau
if external_force is not None:
self.ddy[d] += external_force[d]
self.dy[d] += self.ddy[d] * tau * self.dt * error_coupling
self.y[d] += self.dy[d] * tau * self.dt * error_coupling
return self.y, self.dy, self.ddy, psi
from cs import CanonicalSystem
import numpy as np
import matplotlib.pyplot as plt
# Discrete
# ---------------------------------------------
cs = CanonicalSystem(dt=.001, pattern='discrete')
# test normal rollout
x_track1 = cs.rollout()
cs.reset_state()
# test error coupling
timesteps = int(1.0 / .001)
x_track2 = np.zeros(timesteps)
err = np.zeros(timesteps)
err[200:400] = 2
err_coup = 1.0 / (1 + err)
for i in range(timesteps):
x_track2[i] = cs.step(error_coupling=err_coup[i])
# plot results
fig, ax1 = plt.subplots(figsize=(6, 3))
ax1.plot(x_track1, lw=2)
ax1.plot(x_track2, lw=2)
plt.grid()
plt.legend(['normal rollout', 'error coupling'])
ax2 = ax1.twinx()
ax2.plot(err, 'r-', lw=2)
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
class DMPs(object):
"""Implementation of Dynamic Motor Primitives,
as described in Dr. Stefan Schaal's (2002) paper."""
def __init__(self,
n_dmps,
n_bfs,
decay,
dt=.01,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
"""
n_dmps int: number of dynamic motor primitives
n_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.decay = decay
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps) * y0
self.y0 = np.array(y0)
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps) * goal
self.goal = np.array(goal)
if w is None:
# default is f = 0
w = np.zeros((self.n_dmps, self.n_bfs))
self.w = w
self.ay = np.ones(n_dmps) * 25. if ay is None else ay # Schaal 2012
self.by = self.ay / 4. if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
self.gen_centers()
self.h = np.ones(self.n_bfs) * self.n_bfs**1.5 / self.c / self.cs.ax
# set up the DMP system
self.reset_state()
def check_offset(self):
"""Check to see if initial position and goal are the same
if they are, offset slightly so that the forcing term is not 0"""
for d in range(self.n_dmps):
if (self.y0[d] == self.goal[d]):
self.goal[d] += 1e-4
def gen_centers(self):
"""Set the centre of the Gaussian basis
functions be spaced evenly throughout run time"""
'''x_track = self.cs.discrete_rollout()
t = np.arange(len(x_track))*self.dt
# choose the points in time we'd like centers to be at
c_des = np.linspace(0, self.cs.run_time, self.n_bfs)
self.c = np.zeros(len(c_des))
for ii, point in enumerate(c_des):
diff = abs(t - point)
self.c[ii] = x_track[np.where(diff == min(diff))[0][0]]'''
# desired activations throughout time
des_c = np.linspace(0, self.cs.run_time, self.n_bfs)
self.c = np.ones(len(des_c))
for n in range(len(des_c)):
# finding x for desired times t
self.c[n] = np.exp(-self.cs.ax * des_c[n])
def gen_front_term(self, x, dmp_num):
"""Generates the diminishing front term on
the forcing term.
x float: the current value of the canonical system
dmp_num int: the index of the current dmp
"""
return x * (self.goal[dmp_num] - self.y0[dmp_num])
def gen_goal(self, y_des):
"""Generate the goal for path imitation.
For rhythmic DMPs the goal is the average of the
desired trajectory.
y_des np.array: the desired trajectory to follow
"""
return np.copy(y_des[:, -1])
def gen_psi(self, x):
"""Generates the activity of the basis functions for a given
canonical system rollout.
x float, array: the canonical system state or path
"""
if isinstance(x, np.ndarray):
x = x[:, None]
return np.exp(-self.h * (x - self.c)**2)
def gen_weights(self, f_target):
"""Generate a set of weights over the basis functions such
that the target forcing term trajectory is matched.
f_target np.array: the desired forcing term trajectory
"""
# calculate x and psi
x_track = self.cs.rollout()
psi_track = self.gen_psi(x_track)
# efficiently calculate BF weights using weighted linear regression
self.w = np.zeros((self.n_dmps, self.n_bfs))
for d in range(self.n_dmps):
# spatial scaling term
k = (self.goal[d] - self.y0[d])
for b in range(self.n_bfs):
numer = np.sum(x_track * psi_track[:, b] * f_target[:, d])
denom = np.sum(x_track**2 * psi_track[:, b])
self.w[d, b] = numer / (k * denom)
self.w = np.nan_to_num(self.w)
def save_weight(self, name):
np.save(
os.path.dirname(__file__) + "/PathData/" + name + ".npy", self.w)
def load_weight(self, name):
self.w = np.load(
os.path.dirname(__file__) + "/PathData/" + name + ".npy")
def imitate_path(self, y_des, plot=False):
"""Takes in a desired trajectory and generates the set of
system parameters that best realize this path.
y_des list/array: the desired trajectories of each DMP
should be shaped [n_dmps, run_time]
"""
# set initial state and goal
if y_des.ndim == 1:
y_des = y_des.reshape(1, len(y_des))
self.y0 = y_des[:, 0].copy()
self.y_des = y_des.copy()
self.goal = self.gen_goal(y_des)
self.check_offset()
# generate function to interpolate the desired trajectory
import scipy.interpolate
path = np.zeros((self.n_dmps, self.timesteps))
x = np.linspace(0, self.cs.run_time, y_des.shape[1])
for d in range(self.n_dmps):
path_gen = scipy.interpolate.interp1d(x, y_des[d])
for t in range(self.timesteps):
path[d, t] = path_gen(t * self.dt)
y_des = path
# calculate velocity of y_des
dy_des = np.diff(y_des) / self.dt
# add zero to the beginning of every row
dy_des = np.hstack((np.zeros((self.n_dmps, 1)), dy_des))
# calculate acceleration of y_des
ddy_des = np.diff(dy_des) / self.dt
# add zero to the beginning of every row
ddy_des = np.hstack((np.zeros((self.n_dmps, 1)), ddy_des))
f_target = np.zeros((y_des.shape[1], self.n_dmps))
# find the force required to move along this trajectory
for d in range(self.n_dmps):
f_target[:, d] = (ddy_des[d] - self.ay[d] *
(self.by[d] *
(self.goal[d] - y_des[d]) - dy_des[d]))
# efficiently generate weights to realize f_target
self.gen_weights(f_target)
if plot is True:
# plot the basis function activations
import matplotlib.pyplot as plt
plt.figure()
plt.subplot(211)
psi_track = self.gen_psi(self.cs.rollout())
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()
self.reset_state()
return y_des
def dmp_rollout(self, has_eps, tau=1.0, **kwargs):
"""Generate a system trial, no feedback is incorporated."""
self.reset_state()
self.check_offset()
timesteps = self.timesteps
# set up tracking vectors
y_track = np.zeros((timesteps, self.n_dmps))
dy_track = np.zeros((timesteps, self.n_dmps))
ddy_track = np.zeros((timesteps, self.n_dmps))
basis_track = np.zeros((timesteps, self.n_dmps, self.n_bfs))
eps_track = np.zeros((timesteps, self.n_dmps, self.n_bfs))
f_track = np.zeros((timesteps, self.n_dmps))
int_track = np.zeros((timesteps, self.n_dmps))
for t in range(timesteps):
# run and record timestep
y_track[t], dy_track[t], ddy_track[t], basis_track[t], eps_track[
t], f_track[t] = self.step(has_eps, tau=tau, **kwargs)
for t in range(timesteps - 1):
int_track[t] = 0.5 * ((y_track[t + 1] - y_track[t]) / self.dt -
f_track[t])**2 * self.dt
return y_track, dy_track, ddy_track, basis_track, eps_track, int_track
def reset_state(self):
"""Reset the system state"""
self.y = self.y0.copy()
self.dy = np.zeros(self.n_dmps)
self.ddy = np.zeros(self.n_dmps)
self.b = np.zeros((self.n_dmps, self.n_bfs))
self.cs.reset_state()
def step(self, has_eps, tau=1.0, error=0.0, external_force=None):
"""Run the DMP system for a single timestep.
tau float: scales the timestep
increase tau to make the system execute faster
error float: optional system feedback
"""
error_coupling = 1.0 / (1.0 + error)
# run canonical system
x = self.cs.step(tau=tau, error_coupling=error_coupling)
# generate basis function activation
psi = self.gen_psi(x)
# add epsilon for weight
if has_eps:
self.eps = np.random.randn(self.n_dmps, self.n_bfs) * self.decay
else:
self.eps = np.zeros([self.n_dmps, self.n_bfs])
f_o = np.zeros(self.n_dmps)
for d in range(self.n_dmps):
# generate the forcing term
f = (self.gen_front_term(x, d) *
(np.dot(psi, self.w[d] + self.eps[d])) / np.sum(psi))
f_o[d] = f
# basis term
self.b[d] = self.gen_front_term(x, d) * psi / np.sum(psi)
#self.b[d] = psi
# DMP acceleration
self.ddy[d] = (self.ay[d] *
(self.by[d] *
(self.goal[d] - self.y[d]) - self.dy[d] / tau) +
f) * tau
if external_force is not None:
self.ddy[d] += external_force[d]
self.dy[d] += self.ddy[d] * tau * self.dt * error_coupling
self.y[d] += self.dy[d] * self.dt * error_coupling
return self.y, self.dy, self.ddy, self.b, self.eps, f_o
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 DMPs(object):
"""
Implementation of Dynamic Motor Primitives, as described in Dr. Stefan Schaal's (2002) paper.
"""
def __init__(self,
nDMPs,
nBFs,
dt=0.01,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
"""
Default values from Schaal (2012)
[INPUT]
[VAR NAME] [TYPE] [DESCRIPTION]
(1) nDMPs int Number of dynamic motor primitives
(2) nBFs int Number of basis functions per DMP
(3) dt float Timestep for simulation
(4) y0 list Initial state of DMPs
(5) goal list Goal state of DMPs
(6) w list Tunable parameters, control amplitude of basis functions
(7) ay int Gain on attractor term y dynamics
(8) by int Gain on attractor term y dynamics
"""
self.nDMPs = nDMPs
self.nBFs = nBFs
self.dt = dt
self.y0 = y0 * np.ones(self.nDMPs) if isinstance(y0,
(int, float)) else y0
self.goal = goal * np.ones(self.nDMPs) if isinstance(
goal, (int, float)) else goal
self.w = np.zeros((self.nDMPs, self.nBFs)) if w is None else w
self.ay = np.ones(nDMPs) * 25.0 if ay is None else ay # Schaal 2012
self.by = self.ay / 4.0 if by is None else by # Schaal 2012, 0.25 makes it a critically damped system
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.runTime / self.dt)
# set up the DMP system
self.reset_state()
def check_offset(self):
"""
Check to see if initial position and goal are the same
if they are, offset slightly so that the forcing term is not 0
"""
for d in range(self.nDMPs):
if abs(self.y0[d] - self.goal[d]) < 1e-4:
self.goal[d] += 1e-4
def gen_front_term(self, x, dmpNum):
raise NotImplementedError()
def gen_goal(self, y_des):
raise NotImplementedError()
def gen_psi(self):
raise NotImplementedError()
def gen_weights(self, f_target):
raise NotImplementedError()
def imitatePath(self, y_des, isPlot=False):
"""
Takes in a desired trajectory and generates the set of system parameters that best realize this path.
[INPUT]
[VAR NAME] [TYPE] [DESCRIPTION]
(1) y_des list/array The desired trajectories of each DMP should be shaped [nDMPs, runTime]
"""
# set initial state and goal
if y_des.ndim == 1:
y_des = y_des.reshape(1, len(y_des))
self.y0 = y_des[:, 0].copy()
self.y_des = y_des.copy()
self.goal = self.gen_goal(y_des)
# self.check_offset()
path = np.zeros((self.nDMPs, self.timesteps))
x = np.linspace(0, self.cs.runTime, y_des.shape[1])
for d in range(self.nDMPs):
path_gen = scipy.interpolate.interp1d(x, y_des[d])
for t in range(self.timesteps):
path[d, t] = path_gen(t * self.dt)
y_des = path
dy_des = np.gradient(
y_des, axis=1
) / self.dt # Calculate velocity of y_des with central differences
ddy_des = np.gradient(
dy_des, axis=1
) / self.dt # Calculate acceleration of y_des with central differences
f_target = np.zeros((y_des.shape[1], self.nDMPs))
# find the force required to move along this trajectory
for d in range(self.nDMPs):
f_target[:, d] = ddy_des[d] - self.ay[d] * (
self.by[d] * (self.goal[d] - y_des[d]) - dy_des[d])
# efficiently generate weights to realize f_target
self.gen_weights(f_target)
if isPlot:
# plot the basis function activations
plt.figure()
plt.subplot(211)
psi_track = self.gen_psi(self.cs.rollout())
plt.plot(psi_track)
plt.title("Basis Functions")
# plot the desired forcing function vs approx
for i in range(self.nDMPs):
plt.subplot(2, self.nDMPs, self.nDMPs + 1 + i)
plt.plot(f_target[:, i], "--", label="f_target %i" % i)
for i in range(self.nDMPs):
plt.subplot(2, self.nDMPs, self.nDMPs + 1 + i)
print("w shape: ", self.w.shape)
plt.plot(np.sum(psi_track * self.w[i], axis=1) * self.dt,
label="w*psi %i" % i)
plt.legend()
plt.title("DMP forcing function")
plt.tight_layout()
plt.show()
self.reset_state()
return y_des
def rollout(self, timesteps=None, **kwargs):
"""
Generate a system trial, no feedback is incorporated.
"""
self.reset_state()
if timesteps is None:
timesteps = int(
self.timesteps /
kwargs["tau"]) if "tau" in kwargs else self.timesteps
# set up tracking vectors
yTrack = np.zeros((timesteps, self.nDMPs))
dyTrack = np.zeros((timesteps, self.nDMPs))
ddyTrack = np.zeros((timesteps, self.nDMPs))
for t in range(timesteps):
yTrack[t], dyTrack[t], ddyTrack[t] = self.step(
**kwargs) # run and record timestep
return yTrack, dyTrack, ddyTrack
def reset_state(self):
"""
Reset the system state
"""
self.y = self.y0.copy()
self.dy = np.zeros(self.nDMPs)
self.ddy = np.zeros(self.nDMPs)
self.cs.reset_state()
def step(self, tau=1.0, error=0.0, externalForce=None):
"""
Run the DMP system for a single timestep.
[INPUT]
[VAR NAME] [TYPE] [DESCRIPTION]
tau float Scales the timestep increase tau to make the system execute faster
error float Optional system feedback
"""
errorCoupling = 1.0 / (1.0 + error)
# run canonical system
x = self.cs.step(tau=tau, errorCoupling=errorCoupling)
# generate basis function activation
psi = self.gen_psi(x)
for d in range(self.nDMPs):
# generate the forcing term
f = self.gen_front_term(x, d) * (np.dot(psi,
self.w[d])) / np.sum(psi)
# DMP acceleration
# DMP equation is as following:
# tau * y'' + ay * y' + ayby * (y-g) = f
self.ddy[d] = (self.ay[d] *
(self.by[d] *
(self.goal[d] - self.y[d]) - self.dy[d]) + f)
if externalForce is not None:
self.ddy[d] += externalForce[d]
self.dy[d] += self.ddy[d] * tau * self.dt * errorCoupling
self.y[d] += self.dy[d] * tau * self.dt * errorCoupling
return self.y, self.dy, self.ddy
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):
"""
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(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
# set up the DMP system
self.reset_state()
def check_offset(self):
"""Check to see if initial position and goal are the same
if they are, offset slightly so that the forcing term is not 0"""
for d in range(self.dmps):
if (self.y0[d] == self.goal[d]):
self.goal[d] += 1e-4
def gen_front_term(self, x, dmp_num):
raise NotImplementedError()
def gen_goal(self, y_des):
raise NotImplementedError()
def gen_psi(self):
raise NotImplementedError()
def gen_weights(self, f_target):
raise NotImplementedError()
def imitate_path(self, y_des):
"""Takes in a desired trajectory and generates the set of
system parameters that best realize this path.
y_des list/array: the desired trajectories of each DMP
should be shaped [dmps, run_time]
"""
# set initial state and goal
if y_des.ndim == 1:
y_des = y_des.reshape(1, len(y_des))
self.y0 = y_des[:, 0].copy()
self.y_des = y_des.copy()
self.goal = self.gen_goal(y_des)
self.check_offset()
# calculate velocity of y_des
dy_des = np.diff(y_des) / self.dt
# add zero to the beginning of every row
dy_des = np.hstack((np.zeros((self.dmps, 1)), dy_des))
# calculate acceleration of y_des
ddy_des = np.diff(dy_des) / self.dt
# add zero to the beginning of every row
ddy_des = np.hstack((np.zeros((self.dmps, 1)), ddy_des))
f_target = np.zeros((y_des.shape[1], self.dmps))
# find the force required to move along this trajectory
for d in range(self.dmps):
f_target[:,d] = ddy_des[d] - self.ay[d] * \
(self.by[d] * (self.goal[d] - y_des[d]) - \
dy_des[d])
# efficiently generate weights to realize f_target
self.gen_weights(f_target)
self.reset_state()
return y_des
def rollout(self, timesteps=None, **kwargs):
"""Generate a system trial, no feedback is incorporated."""
self.reset_state()
timesteps = self.timesteps
# set up tracking vectors
y_track = np.zeros((timesteps, self.dmps))
#dy_track = np.zeros((timesteps, self.dmps))
#ddy_track = np.zeros((timesteps, self.dmps))
if self.psi_track is None:
self.psi_track = self.gen_psi(self.cs.x_track)
for t in range(timesteps):
y = self.step(x=self.cs.x_track[t],
psi=self.psi_track[t],
**kwargs)
# record timestep
y_track[t] = y
#dy_track[t] = dy
#ddy_track[t] = ddy
return y_track #, dy_track, ddy_track
def reset_state(self):
"""Reset the system state"""
self.y = self.y0.copy()
self.dy = np.zeros(self.dmps)
self.ddy = np.zeros(self.dmps)
self.cs.reset_state()
def step(self, tau=1.0, state_fb=None, x=None, psi=None):
"""Run the DMP system for a single timestep.
tau float: scales the timestep
increase tau to make the system execute faster
state_fb np.array: optional system feedback
"""
# run canonical system
#cs_args = {'tau':tau,
# 'error_coupling':1.0}
#x = self.cs.step(**cs_args)
#x = x or self.cs.step(**cs_args)
# generate basis function activation
#psi = np.exp(-self.h * (x - self.c)**2)
xpsi_sum = x / np.sum(psi)
dt2 = self.dt * self.dt
for d in range(self.dmps):
# generate the forcing term
f = xpsi_sum * (np.dot(psi, self.w[d]))
# DMP acceleration
self.ddy[d] = (self.ay[d] *
(self.by[d] *
(self.goal[d] - self.y[d]) - self.dy[d]) + f)
#self.dy[d] += self.ddy[d] * self.dt
self.y[d] += self.ddy[d] * dt2
return self.y
