def approx_lwr_batch(self): model = LWR(nb_features=10) self.make_nonlinear_batch_data() start = time.process_time() model.train_lwls(self.x_data, self.y_data) print("LWR time:", time.process_time() - start) model.plot(self.x_data, self.y_data)
def perform_new_dmp(given_traj, initial=None, end=None, duration=1.0, dt=0.001, use_improved=True): traj_1d = np.reshape(given_traj, np.size(given_traj)) list_traj = traj_1d.tolist() if not initial: initial = list_traj[0] if not end: end = list_traj[-1] while (hasattr(initial, "__getitem__")): initial = initial[0] while (hasattr(end, "__getitem__")): end = end[0] traj_freq = int(duration / dt) traj = DiscreteDMP.compute_derivatives(list_traj, traj_freq) traj = list(traj) dmp = DiscreteDMP(improved_version=use_improved, reg_model=LWR(activation=0.1, exponentially_spaced=True, n_rfs=20)) dmp.learn_batch(traj, traj_freq) dmp_adapt = DiscreteDMP( improved_version=use_improved, reg_model=dmp.lwr_model) #copy.deepcopy(dmp.lwr_model)) dmp_adapt._is_learned = True dmp.delta_t = dt dmp.setup(traj_1d[0], traj_1d[-1], duration) dmp_adapt.delta_t = dt dmp_adapt.setup(initial, end, duration) traj_reproduced = [] traj_adapted = [] for _ in range(int(dmp.tau / dmp.delta_t)): dmp.run_step() dmp_adapt.run_step() traj_reproduced.append(dmp.x) traj_adapted.append(dmp_adapt.x) traj_reproduced = np.array(traj_reproduced).reshape(np.size(given_traj)) print(np.size(traj_adapted)) traj_adapted = np.array(traj_adapted).reshape(np.size(given_traj)) print(np.size(traj_adapted)) return [traj_adapted, traj_reproduced]
def main(): # start and goal of the movement (1-dim) start = 0.5 goal = 1.0 ####### generate a trajectory (minimum jerk) # duration duration = 1.0 # time steps delta_t = 0.001 # trajectory is a list of 3-tuples with (pos,vel,acc) traj = min_jerk_traj(start, goal, 1.0, delta_t) traj_freq = int(1.0 / delta_t) ####### learn DMP dmp = DiscreteDMP( False, LWR(activation=0.3, exponentially_spaced=False, n_rfs=8, use_offset=True)) dmp.learn_batch(traj, traj_freq) ####### learn DMP # setup DMP with start and goal dmp.setup(start + 0.4, goal + 0.2, duration) # trajectory reproduced_traj = [] # states of canonical system (plotting) s = [] s_time = [] # run steps (for each point of the sample trajectory) for _ in xrange(int(dmp.tau / dmp.delta_t)): # change goal while execution #if x == 500: # dmp.goal = 4.0 # run a single integration step dmp.run_step() # remember canonical system values s.append(dmp.s) s_time.append(dmp.s_time) # save reproduced trajectory reproduced_traj.append([dmp.x, dmp.xd, dmp.xdd]) ####### PLOTTING fig_pos = plt.figure('dmp batch learn from min jerk', figsize=(7, 5)) ax_pos2 = fig_pos.add_subplot(111) plot_time = np.arange(len(traj)) * delta_t ax_pos2.plot(plot_time, np.asarray(traj)[:, 0], label='demonstration') ax_pos2.plot(plot_time, np.asarray(reproduced_traj)[:, 0], label='adapted reproduction', linestyle='dashed') ax_pos2.legend(loc='upper left') plt.show() # create figure fig = plt.figure('dmp batch learn from min jerk', figsize=(16, 4.6)) # create axes ax_pos = fig.add_subplot(131) ax_vel = fig.add_subplot(132) ax_acc = fig.add_subplot(133) # plot on the axes plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc), traj, delta_t, label='demonstration') plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc), reproduced_traj, dmp.delta_t, label='reproduction', linestyle='dashed') fig.tight_layout() plt.show() fig = plt.figure('ftarget', figsize=(16, 4.6)) # plot ftarget (real and predicted) ax_ft = fig.add_subplot(131) ax_ft.plot(dmp.target_function_input, dmp.target_function_ouput, linewidth=2, label=r'$f_{target}(s)$') ax_ft.plot(dmp.target_function_input, dmp.target_function_predicted, '--', linewidth=2, label=r'$f_{predicted}(s)$') #dashes=(3, 3) ax_ft.set_xlabel(r'$s$') ax_ft.set_ylabel(r'$f(s)$') ax_ft.legend() # kernels (lwr) ax_kernel = fig.add_subplot(132) dmp.lwr_model.plot_kernels(ax_kernel) ax_kernel.set_xlabel(r'$s$') ax_kernel.set_ylabel(r'$\psi(s)$') # canonical system # ax_cs = fig.add_subplot(236) # ax_cs.plot(s_time, s) # ax_cs.set_xlabel('Time [s]') # ax_cs.set_ylabel(r'$s$') # weights of kernels (w) ax_w = fig.add_subplot(133) ax_w.set_xlabel(r'$s$') ax_w.set_ylabel(r'$f(s)$') dmp.lwr_model.plot_linear_models(ax_w) # lwr_wights = dmp.lwr_model.get_thetas() # ax_w.bar(range(len(lwr_wights)), lwr_wights, width=0.3) # ax_w.set_xlabel('$w_i$') # ax_w.set_ylabel('') fig.tight_layout() plt.show()
def __init__(self, transf_system, reg_model=None): self.ts = transf_system # state variables # ----------------------------- self.start = 0.0 self.goal = 1.0 self.tau = 1.0 # Cannonical System # ----------------------------- # s_min self.cutoff = 0.001 # alpha (canonical system parameter) self.alpha = abs(math.log(self.cutoff)) # Canonical System self.s = 1.0 # canonical system is starting with 1.0 self.s_time = 0.0 # Attractor # ----------------------------- # spring term self.K = 50.0 # damping term self.D = self.K / 4 # time steps (for the run) self.delta_t = 0.001 # Transformation System # ---------------------------- # current position, velocity, acceleration self.x = 0.0 self.xd = 0.0 self.xdd = 0.0 # internal variables ''' xd not scaled by tau aka v''' self._raw_xd = 0.0 self.f = 0 # target function input (x) and output (y) self.target_function_input = None self.Fd = None # debugging (y values predicted by fitted lwr model) self.Fp = None # do not predict f by approximated function, # use values of ft directly only works if duration is 1.0!! self.use_ft = False # create LWR model and set parameters # ----------------------------- if not reg_model: # default regression model self.lwr_model = LWR(activation=0.1, exponentially_spaced=True, n_rfs=20) else: # custom regression model self.lwr_model = reg_model # is the DMP initialized? self._is_learned = False
def main(): # duration duration = 1.0 # adaption offset adapt_offset = +0.02 # time steps delta_t = 0.001 # load position trajectory traj_pos = json.load(open("traj_full.json", 'r'))["x"][2000:8000][::6] #[5000:8000][::3] # rest start and goal position out of trajectory start = traj_pos[0] goal = traj_pos[-1] traj_freq = int(1.0 / delta_t) # calc it here for easier drawing traj = DiscreteDMP.compute_derivatives(traj_pos, traj_freq) print(type(traj)) traj = list(traj) ####### learn DMP dmp = DiscreteDMP(True, reg_model=LWR(activation=0.1, exponentially_spaced=True, n_rfs=20)) #dmp.use_ft = True dmp.learn_batch(traj, traj_freq) dmp_adapt = DiscreteDMP(True, reg_model=dmp.lwr_model) #copy.deepcopy(dmp.lwr_model)) dmp_adapt._is_learned = True ####### learn DMP # setup DMP with start and goal dmp.delta_t = delta_t dmp.setup(start, goal, duration) dmp_adapt.delta_t = delta_t dmp_adapt.setup(start, goal + adapt_offset, duration) # trajectory traj_reproduced = [] traj_adapted = [] # states of canonical system (plotting) s = [] s_time = [] # run steps (for each point of the sample trajectory) for _ in range(int(dmp.tau / dmp.delta_t)): # change goal while execution #if x == 500: # dmp.goal = 4.0 # run a single integration step dmp.run_step() dmp_adapt.run_step() # remember canonical system values s.append(dmp.s) s_time.append(dmp.s_time) # save reproduced trajectory traj_reproduced.append([dmp.x, dmp.xd, dmp.xdd]) traj_adapted.append([dmp_adapt.x, dmp_adapt.xd, dmp_adapt.xdd]) ####### PLOTTING # create figure fig = plt.figure('dmp', figsize=(6, 4)) # create axes ax_pos = fig.add_subplot(111) # plot on the axes #ax_pos.plot(np.arange(0,1,0.001), traj_pos) plot_time = np.arange(len(traj)) * delta_t ax_pos.plot(plot_time, np.asarray(traj)[:, 0], label='demonstration') ax_pos.plot(plot_time, np.asarray(traj_reproduced)[:, 0], label='reproduction', linestyle='dashed') ax_pos.plot(plot_time, np.asarray(traj_adapted)[:, 0], label='adapted (%+0.2f)' % adapt_offset) ax_pos.legend(loc='lower left') # plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc), traj, dmp.delta_t, label='demonstration') # plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc), traj_reproduced, dmp.delta_t, label='reproduction', linestyle='dashed') # plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc), traj_adapted, dmp.delta_t, label='adapted (+0.02)') fig.tight_layout() plt.show() # fig2 = plt.figure('lwr', figsize=(6, 4)) # ax_ft = fig2.add_subplot(211) ax_ft.plot(dmp.target_function_input, dmp.target_function_ouput, linewidth=2, label=r'$f_{target}(s)$') ax_ft.plot(dmp.target_function_input, dmp.target_function_predicted, '--', linewidth=2, label=r'$f_{predicted}(s)$') #dashes=(3, 3) plt.show()
def perform_new_dmp_adapted(given_traj, initial=None, end=None, duration=1.0, dt=None, use_improved=True, k=None, d=None): traj_1d = np.reshape(given_traj, np.size(given_traj)) list_traj = traj_1d.tolist() if not initial: initial = list_traj[0] if not end: end = list_traj[-1] if (isinstance(initial, (list, np.ndarray))): print(initial) initial = initial[0] if (isinstance(end, (list, np.ndarray))): end = end[0] traj_freq = int(np.size(given_traj)) if not dt: dt = duration / traj_freq traj = DiscreteDMP.compute_derivatives(list_traj, traj_freq) traj = list(traj) dmp = DiscreteDMP(improved_version=use_improved, reg_model=LWR(activation=0.1, exponentially_spaced=True, n_rfs=20), K=k, D=d) dmp.learn_batch(traj, traj_freq) dmp_adapt = DiscreteDMP(improved_version=use_improved, reg_model=dmp.lwr_model, K=k, D=d) #copy.deepcopy(dmp.lwr_model)) dmp_adapt._is_learned = True dmp.delta_t = dt dmp.setup(traj_1d[0], traj_1d[-1], duration) dmp_adapt.delta_t = dt dmp_adapt.setup(initial, end, duration) traj_reproduced = [] traj_adapted = [] for _ in range(int(dmp.tau / dmp.delta_t)): dmp.run_step() dmp_adapt.run_step() traj_reproduced.append(dmp.x) traj_adapted.append(dmp_adapt.x) #print(np.size(traj_adapted)) traj_adapted = np.array(traj_adapted).reshape(np.size(given_traj)) #print(np.size(traj_adapted)) #print(traj_adapted) return traj_adapted #def perform_dmp_improved(traj, initial=[], end=[], ay=None, by=None): # #set up endpoints if none specified # if not initial: # initial = traj[0] # if not end: # end = traj[max(np.shape(traj)) - 1] # #transpose if necessary # if len(np.shape(traj)) > 1: # if np.shape(traj)[0] > np.shape(traj)[1]: # traj = np.transpose(traj) # ntraj = np.reshape(traj, (1, max(np.shape(traj)))) # ## DMP ## # dmp_traj = perform_dmp(ntraj, [initial, end], alpha_y=ay, beta_y=by) # dmp_traj = np.reshape(dmp_traj, np.size(traj)) # return dmp_traj
def __init__(self, improved_version=False, reg_model=None): ## time constants choosen for critical damping # s_min self.cutoff = 0.001 # alpha (canonical system parameter) self.alpha = abs(math.log(self.cutoff)) # spring term self.k_gain = 50.0 # damping term self.d_gain = self.k_gain / 4 # time steps (for the run) self.delta_t = 0.001 ## state variables '''start position aka $x_0$''' self.start = 0.0 '''goal position aka $g$''' self.goal = 1.0 '''movement duration: temporal scaling factor $\tau$''' self.tau = 1.0 ## Transformation System # current position, velocity, acceleration '''$x$ (position)''' self.x = 0.0 '''$\dot x$ aka v (velocity)''' self.xd = 0.0 '''$\ddot x$ aka \dot v (acceleration)''' self.xdd = 0.0 # internal variables ''' xd not scaled by tau aka v''' self._raw_xd = 0.0 '''current value of function f (perturbation function)''' self.f = 0.0 # target function input (x) and output (y) self.target_function_input = None self.target_function_ouput = None # debugging (y values predicted by fitted lwr model) self.target_function_predicted = None # do not predict f by approximated function, use values of ft directly - only works if duration is 1.0!! self.use_ft = False # create LWR model and set parameters if not reg_model: # default regression model self.lwr_model = LWR(activation=0.1, exponentially_spaced=True, n_rfs=20) else: # custom regression model self.lwr_model = reg_model # Canonical System self.s = 1.0 # canonical system is starting with 1.0 self.s_time = 0.0 # is the DMP initialized? self._initialized = False self._is_learned = False # set the correct transformation and ftarget functions if improved_version: self._transformation_func = self._transformation_func_improved self._ftarget_func = self._ftarget_func_improved else: self._transformation_func = self._transformation_func_original self._ftarget_func = self._ftarget_func_original
def perform_new_dmp_adapted(given_traj, initial=None, end=None, duration=1.0, dt=None, use_improved=True, k=None, d=None): traj_1d = np.reshape(given_traj, np.size(given_traj)) list_traj = traj_1d.tolist() if not initial: initial = list_traj[0] if not end: end = list_traj[-1] if (isinstance(initial, (list, np.ndarray))): print(initial) initial = initial[0] if (isinstance(end, (list, np.ndarray))): end = end[0] traj_freq = int(np.size(given_traj)) if not dt: dt = duration / traj_freq traj = DiscreteDMP.compute_derivatives(list_traj, traj_freq) traj = list(traj) dmp = DiscreteDMP(improved_version=use_improved, reg_model=LWR(activation=0.1, exponentially_spaced=True, n_rfs=20), K=k, D=d) dmp.learn_batch(traj, traj_freq) dmp_adapt = DiscreteDMP(improved_version=use_improved, reg_model=dmp.lwr_model, K=k, D=d) #copy.deepcopy(dmp.lwr_model)) dmp_adapt._is_learned = True dmp.delta_t = dt dmp.setup(traj_1d[0], traj_1d[-1], duration) dmp_adapt.delta_t = dt dmp_adapt.setup(initial, end, duration) traj_reproduced = [] traj_adapted = [] for _ in range(int(dmp.tau / dmp.delta_t)): dmp.run_step() dmp_adapt.run_step() traj_reproduced.append(dmp.x) traj_adapted.append(dmp_adapt.x) #print(np.size(traj_adapted)) traj_adapted = np.array(traj_adapted).reshape(np.size(given_traj)) #print(np.size(traj_adapted)) #print(traj_adapted) return traj_adapted