def augmented_lagrangian_method(initial_lagrange_multiplier, initial_penalty, initial_tolerance, global_tolerance, max_iter, robot, generate_initial_guess=True, convergence_analysis=False): objective_function = generate_objective_function(robot) lagrange_multiplier_function = generate_lagrange_multiplier(robot) if generate_initial_guess is True: thetas = robot.generate_initial_guess() elif generate_initial_guess == "random": thetas = np.random.rand(robot.n * robot.s) * 2 * np.pi else: thetas = np.zeros(robot.n * robot.s) mu = initial_penalty tolerance = initial_tolerance lagrange_multiplier = initial_lagrange_multiplier # Saving the iterates if convergence analysis is active if convergence_analysis is True: iterates = thetas.copy().reshape((robot.n * robot.s, 1)) print("Starting augmented lagrangian method") for i in range(max_iter): augmented_lagrangian_objective = generate_augmented_lagrangian_objective( robot, lagrange_multiplier, mu) augmented_lagrangian_objective_gradient = generate_augmented_lagrangian_objective_gradient( robot, lagrange_multiplier, mu) thetas = BFGS(thetas, augmented_lagrangian_objective, augmented_lagrangian_objective_gradient, tolerance) lagrange_multiplier = lagrange_multiplier_function( thetas, lagrange_multiplier, mu) mu *= 1.4 tolerance *= 0.7 # Adding the new thetas to iterates used for convergence analysis if convergence_analysis is True: iterates = np.concatenate( (iterates, thetas.reshape(robot.n * robot.s, 1)), axis=1) current_norm = np.linalg.norm( augmented_lagrangian_objective_gradient(thetas)) if current_norm < global_tolerance: path_figure(thetas.reshape((robot.n, robot.s), order='F'), robot, show=True) print('The objective function evaluates to:', objective_function(thetas)) print("Augmented lagrangian method successful") if convergence_analysis is True: return iterates, generate_objective_function(robot) else: return thetas print("Augmented lagrangian method unsuccessful")
def test_history(self): x0 = -np.array([1.3, 2.7]) opt = BFGS(self.oracle, x0, line_search_options={ 'method': 'Constant', 'c': 1.0 }, tolerance=1e-6) opt.run(10) x_min = opt.hist['x_star'] func_steps = [25.635000000000005, 22.99, -9.48707349065929, -9.5] grad_norm_steps = [11.629703349613008, 11.4, 0.22738961577617722, 0.0] time_steps = [0.0] * 4 # Dummy values x_steps = [ np.array([-1.3, -2.7]), np.array([1.0, 8.7]), np.array([1.0, 2.88630519]), np.array([1., 3.]) ] true_history = dict(grad_norm=grad_norm_steps, time=time_steps, x=x_steps, func=func_steps) check_equal_histories(opt.hist, true_history)
def test_quality(self): opt = BFGS(self.oracle, self.x0, tolerance=1e-5) opt.run(5) x_min = opt.hist['x_star'] # x_min, message, _ = LBFGS(self.oracle, self.x0, tolerance=1e-5) f_min = self.oracle.func(x_min) g_k_norm_sqr = norm(self.A.dot(x_min) - self.b, 2)**2 g_0_norm_sqr = norm(self.A.dot(self.x0) - self.b, 2)**2 self.assertLessEqual(g_k_norm_sqr, 1e-5 * g_0_norm_sqr) self.assertLessEqual(abs(f_min - self.f_star), 1e-5 * g_0_norm_sqr)
def quadratic_penalty_method(robot_arm): n = robot_arm.n s = robot_arm.s Q = generate_quadratically_penalized_objective(robot_arm) grad_Q = generate_quadratically_penalized_objective_gradient(robot_arm) thetas = robot_arm.generate_initial_guess(show=False) mu = 1 tau = 1e-4 def get_f(mu): return partial(Q, mu=mu) def get_grad_f(mu): return partial(grad_Q, mu=mu) print('Starting quadratic penalty method') for yolo in range(1000): f = get_f(mu) grad_f = get_grad_f(mu) try: thetas = BFGS(thetas, f, grad_f, tau) except MaximumIterationError: print('Reached MaximumIterationError in loop number {}'.format(yolo)) break mu = 1.5 * mu tau = 0.5 * tau return thetas
def test_line_search_options(self): """Check if argument `line_search_options` is supported.""" BFGS(self.oracle, self.x0, line_search_options={ 'method': 'Wolfe', 'c1': 1e-4, 'c2': 0.9 })
def calculate_joint_angles(self, p, theta=None): ''' Takes in a destination p and an initial guess theta and returns a theta which maps closely to the point p ''' # If theta is not provided, use the zero as initial guess if theta is None: theta = np.zeros(self.n) assert theta.ndim == 1 # Check if the goal is out of reach p_length = np.linalg.norm(p) if p_length >= self.reach: print("Point outside interior of configuration space. Minimum found analytically.") return self.move_closest_to_out_of_reach(p) elif self.annulus and p_length <= self.inner_reach: print("Point outside interior of configuration space. Minimum found analytically.") return self.move_closest_to_out_of_reach(p, internal=True) else: f = self.generate_f(p) gradient = self.generate_gradient(p) return BFGS(theta, f, gradient, self.precision, initial_guess=True)
def test_max_iter(self): """Check if argument `max_iter` is supported.""" opt = BFGS(self.oracle, self.x0) opt.run(max_iter=0)
def test_tolerance(self): """Check if argument `tolerance` is supported.""" BFGS(self.oracle, self.x0, tolerance=1e-5)
def test_default(self): """Check if everything works correctly with default parameters.""" opt = BFGS(self.oracle, self.x0) opt.run()
def run_all_methods(oracle, sketch_sizes, mat, max_iter, output_folder, x_0=None, sigma_tolerance=1e-10, method_tolerance=1e-16, stopping_criteria='func_abs', add_text='', random_state=None, linesearch_methods=['Wolfe'], methods=None, overwrite=False): import os os.system('mkdir -p {}'.format(output_folder)) if methods is None: methods = ['svd', 'svd-no-sigma', 'gauss', 'coord', 'bfgs', 'nesterov'] np.random.seed(random_state) if x_0 is None: x_0 = np.random.normal(loc=0., scale=1., size=mat.shape[1]) add_text = '_{}'.format(add_text) if add_text != '' else '' def run_rbfgs(mat_distr, line_search_options, distr_name, **kwargs): output_file = '{}/rbfgs_{}_linesearch={}{}.pkl'.format( output_folder, distr_name, line_search_options['method'].lower(), add_text) run_rbfgs_experiment(oracle, x_0, mat_distr=mat_distr, sketch_sizes=sketch_sizes, max_iter=max_iter, tolerance=method_tolerance, stopping_criteria=stopping_criteria, output_file=output_file, overwrite=overwrite, **kwargs) if 'svd' in methods or 'svd-no-sigma' in methods: try: with open('{}/svd.pkl'.format(output_folder), 'rb') as file: U, sigma_diag, Vh = pickle.load(file) print('Read SVD from {}/svd.pkl'.format(output_folder)) except FileNotFoundError: print('Computing SVD...', end='') U, sigma_diag, Vh = scipy.linalg.svd(mat.T, full_matrices=False) print('Done') with open('{}/svd.pkl'.format(output_folder), 'wb') as file: pickle.dump((U, sigma_diag, Vh), file) nondeg_count = (sigma_diag > sigma_tolerance).sum() print('Singular values above tolerance: {}'.format(nondeg_count)) print() if 'svd' in methods: print('RBFGS-SVD sketch... ', end='') mat_distr = CustomDiscrete(U[:, :nondeg_count], sort_ids=True) for ls in linesearch_methods: run_rbfgs(mat_distr, {'method': ls}, 'svd') print('Done') if 'svd-no-sigma' in methods: print('RBFGS-SVD sketch no sigma... ', end='') mat_distr = CustomDiscrete(U, sort_ids=True) for ls in linesearch_methods: run_rbfgs(mat_distr, {'method': ls}, 'svd-no-sigma') print('Done') if 'gauss' in methods: print('RBFGS-gauss... ', end='') mat_distr = Gaussian(-1., 1., [mat.shape[1], 1]) for ls in linesearch_methods: run_rbfgs(mat_distr, {'method': ls}, 'gauss') print('Done') if 'coord' in methods: print('RBFGS-coord...', end='') mat_distr = CustomDiscrete(np.eye(mat.shape[1]), sort_ids=True) for ls in linesearch_methods: run_rbfgs(mat_distr, {'method': ls}, 'coord') print('Done') if 'bfgs' in methods: print('BFGS... ', end='') for ls in linesearch_methods: if 'bfgs_linesearch={}{}.pkl'\ .format(ls.lower(), add_text) not in os.listdir(output_folder): method = BFGS(oracle, x_0, tolerance=method_tolerance, stopping_criteria=stopping_criteria, line_search_options={'method': 'Wolfe'}) method.run(max_iter) method.oracle = None method.H_k = None method.x_0 = None with open('{}/bfgs_linesearch={}{}.pkl'\ .format(output_folder, ls.lower(), add_text), 'wb') as file: pickle.dump(method, file) print('Done') print() print('All runs completed.')
def extended_augmented_lagrangian_method(initial_lagrange_multiplier, initial_penalty, initial_tolerance, global_tolerance, max_iter, robot, barrier_penalty, generate_initial_guess=False, convergence_analysis=False): for i in range(robot.s): if not is_close_to_config_space(robot.destinations[:, i], robot.neighbor_tree, robot.lengths): print("Destination points not in configuration space") return None lagrange_multiplier_function = generate_extended_lagrange_multiplier(robot) if generate_initial_guess is True: thetas_slack = np.zeros(3 * robot.n * robot.s, ) thetas_slack[:robot.n * robot.s] = robot.generate_initial_guess() thetas_slack[robot.n * robot.s::2] = thetas_slack[:robot.n * robot. s] + robot.angular_constraint thetas_slack[robot.n * robot.s + 1::2] = robot.angular_constraint - thetas_slack[:robot.n * robot.s] elif generate_initial_guess == "random": thetas_slack = np.zeros(3 * robot.n * robot.s, ) thetas_slack[:robot.n * robot.s] = np.random.rand(robot.n * robot.s) * 2 * np.pi thetas_slack[robot.n * robot.s::2] = thetas_slack[:robot.n * robot. s] + robot.angular_constraint thetas_slack[robot.n * robot.s + 1::2] = robot.angular_constraint - thetas_slack[:robot.n * robot.s] else: thetas_slack = np.zeros(3 * robot.n * robot.s, ) mu = initial_penalty tolerance = initial_tolerance lagrange_multiplier = initial_lagrange_multiplier # Saving the iterates if convergence analysis is active if convergence_analysis is True: iterates = thetas_slack.copy().reshape((3 * robot.n * robot.s, 1)) print("Starting augmented lagrangian method") for i in range(max_iter): augmented_lagrangian_objective = generate_extended_augmented_objective( robot, barrier_penalty, lagrange_multiplier, mu) augmented_lagrangian_objective_gradient = generate_extended_augmented_gradient( robot, barrier_penalty, lagrange_multiplier, mu) previous_thetas_slack = thetas_slack thetas_slack = BFGS(thetas_slack, augmented_lagrangian_objective, augmented_lagrangian_objective_gradient, tolerance) lagrange_multiplier = lagrange_multiplier_function( thetas_slack, lagrange_multiplier, mu) mu *= 5 tolerance *= 0.9 # Adding the new thetas to iterates used for convergence analysis if convergence_analysis is True: iterates = np.concatenate( (iterates, thetas_slack.reshape(robot.n * robot.s, 1)), axis=1) current_norm = np.linalg.norm( augmented_lagrangian_objective_gradient(thetas_slack)) if current_norm < global_tolerance or np.linalg.norm( previous_thetas_slack - thetas_slack) < 0.1: path_figure(thetas_slack[:robot.n * robot.s].reshape( (robot.n, robot.s), order='F'), robot, show=True) print("Augmented lagrangian method successful") if convergence_analysis is True: return iterates, generate_extended_objective_function else: return thetas_slack path_figure(thetas_slack[:robot.n * robot.s].reshape((robot.n, robot.s), order='F'), robot, show=True) print("Augmented lagrangian method unsuccessful")