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(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 DMPs(object): """Implementation of Dynamic Motor Primitives, as described in Dr. Stefan Schaal's (2002) paper.""" def __init__( self, n_dmps, n_bfs, dt=0.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.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 = y0 if isinstance(goal, (int, float)): goal = np.ones(self.n_dmps) * goal self.goal = 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.0 if ay is None else ay # Schaal 2012 self.by = self.ay / 4.0 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.n_dmps): if abs(self.y0[d] - self.goal[d]) < 1e-4: 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, 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 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)) # 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], "--", label="f_target %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, label="w*psi %i" % ii, ) 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: 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 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. 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.n_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]) + 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
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
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 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_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