def main(Vxf0, urdf, options): modelNames = ['w.mat', 'Sshape.mat'] # Two example models provided by Khansari modelNumber = 1 # could be zero or one depending on the experiment the user is running data, demoIdx = load_saved_mat_file(lyap + '/' + 'example_models/' + modelNames[modelNumber]) Vxf0['d'] = int(data.shape[0] / 2) Vxf0.update(Vxf0) Vxf0 = guess_init_lyap(data, Vxf0, options['int_lyap_random']) cost = Cost() while cost.success: print('Optimizing the lyapunov function') Vxf, J = cost.learnEnergy(Vxf0, data, options) old_l = Vxf0['L'] Vxf0['L'] += 1 print('Constraints violated. increasing the size of L from {} --> {}'. format(old_l, Vxf0['L'])) if cost.success: print('optimization succeeded without violating constraints') break # Plot the result of V h1 = plt.plot(data[0, :], data[1, :], 'r.', label='demonstrations') extra = 30 axes_limits = [ np.min(data[0, :]) - extra, np.max(data[0, :]) + extra, np.min(data[1, :]) - extra, np.max(data[1, :]) + extra ] h3 = cost.energyContour(Vxf, axes_limits, np.array(()), np.array(()), np.array(()), False) h2 = plt.plot(0, 0, 'g*', markersize=15, linewidth=3, label='target') plt.title('Energy Levels of the learned Lyapunov Functions', fontsize=12) plt.xlabel('x (mm)', fontsize=15) plt.ylabel('y (mm)', fontsize=15) h = [h1, h2, h3] # Run DS opt_sim = dict() opt_sim['dt'] = 0.01 opt_sim['i_max'] = 4000 opt_sim['tol'] = 1 d = data.shape[0] / 2 # dimension of data x0_all = data[:int(d), demoIdx[0, :-1] - 1] # finding initial points of all demonstrations # get gmm params gmm = GMM(num_clusters=options['num_clusters']) gmm.update(data.T, K=options['num_clusters'], max_iterations=100) mu, sigma, priors = gmm.mu.T, gmm.sigma.T, gmm.logmass.T # rho0 and kappa0 impose minimum acceptable rate of decrease in the energy # function during the motion. Refer to page 8 of the paper for more information rho0 = 1 kappa0 = 0.1 inp = list(range(Vxf['d'])) output = np.arange(Vxf['d'], 2 * Vxf['d']) xd, _ = dsStabilizer(x0_all, Vxf, rho0, kappa0, priors, mu, sigma, inp, output, cost) # Evalute DS xT = np.array([]) d = x0_all.shape[0] # dimension of the model if not xT: xT = np.zeros((d, 1)) # initialization nbSPoint = x0_all.shape[1] x = [] #x0_all[0, 1] = -180 # modify starting point a bit to see performance in further regions #x0_all[1, 1] = -130 x.append(x0_all) xd = [] if xT.shape == x0_all.shape: XT = xT else: XT = np.tile( xT, [1, nbSPoint ]) # a matrix of target location (just to simplify computation) t = 0 # starting time dt = 0.01 for i in range(4000): xd.append( dsStabilizer(x[i] - XT, Vxf, rho0, kappa0, priors, mu, sigma, inp, output, cost)[0]) x.append(x[i] + xd[i] * dt) t += dt for i in range(nbSPoint): # Choose one trajectory x = np.reshape(x, [len(x), d, nbSPoint]) x0 = x[:, :, i] if i == 0: plt.plot(x0[:, 0], x0[:, 1], linestyle='--', label='DS eval', color='blue') else: plt.plot(x0[:, 0], x0[:, 1], linestyle='--', color='blue') plt.legend() plt.show()
def main(Vxf0, urdf, options): gmm = GMM(num_clusters=options['num_clusters']) if options['args'].data_type == 'h5_data': filename = '{}.h5'.format('torobo_processed_data') with h5py.File(filename, 'r+') as f: data = f['data/data'].value logging.debug(''.format(data.shape)) elif options['args'].data_type == 'pipe_et_trumpet': path = 'data' name = 'cart_pos.csv' data, data6d = format_data(path, name, learn_type='2d') if options['use_6d']: data = data6d Vxf0['d'] = int(data.shape[0] / 2) Vxf0.update(Vxf0) Vxf0 = guess_init_lyap(data, Vxf0, options['int_lyap_random']) cost = Cost() # cost.success = False while cost.success: # cost.success = False Vxf, J = cost.learnEnergy(Vxf0, data, options) # if not cost.success: # increase L and restart the optimization process old_l = Vxf0['L'] Vxf0['L'] += 1 rospy.logwarn( 'Constraints violated. increasing the size of L from {} --> {}'. format(old_l, Vxf0['L'])) if cost.success: rospy.logdebug( 'optimization succeeded without violating constraints') break # get gmm params gmm.update(data.T, K=options['num_clusters'], max_iterations=100) mu, sigma, priors = gmm.mu, gmm.sigma, gmm.logmass if options['disp']: logging.debug(' mu {}, sigma {}, priors: {}'.format( mu.shape, sigma.shape, priors.shape)) inp = slice(0, Vxf['d']) #np.array(range(0, Vxf['d'])) out = slice(Vxf['d'], 2 * Vxf['d']) #np.array(range(Vxf['d'], 2* Vxf['d'])) rho0, kappa0 = 1.0, 1.0 gmr_handle = lambda x: GMR(priors, mu, sigma, x, inp, out) stab_handle = lambda dat: dsStabilizer(dat, gmr_handle, Vxf, rho0, kappa0) x0_all = data[0, :] XT = data[-1, :] logger.debug('XT: {} x0: {} '.format(XT.shape, x0_all.shape)) home_pos = [0.0] * 7 executor = ToroboExecutor(home_pos, urdf) x, xd = executor.optimize_traj(data6d, stab_handle, opt_exec)