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_plot_pure_functon(self): # Save values before function invocation original_destinations = self.robot_arm.destinations.copy() original_theta_matrix = self.theta_matrix.copy() # Run the pure function path_figure(self.theta_matrix, self.robot_arm, show=False) # Assert that none of the arguments have been changed np.testing.assert_array_equal(original_destinations, self.robot_arm.destinations) np.testing.assert_array_equal(original_theta_matrix, self.theta_matrix)
def generate_initial_guess(self, show=False, first_configuration=None): # How should the arm be positioned before generating a valid # configuration which moves the arm to the first destination if first_configuration is None: last_theta = self.theta else: assert first_configuration.shape == (self.n,) last_theta = first_configuration # Calculate initial guess for joint angles which hit all the # destinations but not necessarily with little effort self.initial_guess = np.empty((self.n, self.s,)) assert self.initial_guess.shape == (self.n, self.s,) for index, destination in enumerate(self.destinations.T): last_theta = self.calculate_joint_angles(destination, last_theta) self.initial_guess[:, index] = last_theta fig = path_figure(self.initial_guess, self, show=False) fig.suptitle('Initial guess calculated by BFGS') if show is True: plt.show() return self.initial_guess.reshape((self.n * self.s,), order='F')
def plot_movement(self): joint_positions = self.position(joints=True) path = plotting.path_figure(joint_positions, destinations) path.show()
def test_quadratic_penalty_method(self): thetas = quadratic_penalty_method(self.robot_arm) path_figure(thetas.reshape((self.n, self.s,), order='F'), self.robot_arm, show=False)
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")