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