def generate_lagrange_multiplier(robot_arm):
constraints_function = generate_constraints_function(robot_arm)
def lagrange_multiplier(thetas, old_lagrange_multiplier, mu):
return old_lagrange_multiplier - mu * constraints_function(thetas)
return lagrange_multiplier
def generate_quadratically_penalized_objective_gradient(robot_arm):
n = robot_arm.n
s = robot_arm.s
objective_gradient = generate_objective_gradient_function(robot_arm)
constraints_func = generate_constraints_function(robot_arm)
constraint_gradients_func = generate_constraint_gradients_function(robot_arm)
def quadratic_constraint_gradients(thetas):
if not thetas.shape == (n * s,):
raise ValueError('Thetas is not given as 1D-vector, but as: ' + \
str(thetas.shape))
constraint_gradients = constraint_gradients_func(thetas)
constraints = constraints_func(thetas)
assert constraint_gradients.shape == (n*s, 2*s,)
assert constraints.shape == (2*s,)
return constraints.reshape((1, 2*s,)) * constraint_gradients
def quadratically_penalized_objective_gradient(thetas, mu):
if not thetas.shape == (n * s,):
raise ValueError('Thetas is not given as 1D-vector, but as: ' + \
str(thetas.shape))
grad = objective_gradient(thetas)
constraint_gradients = 0.5 * mu * quadratic_constraint_gradients(thetas)
return grad + 0.5 * mu * np.sum(constraint_gradients, axis=1)
return quadratically_penalized_objective_gradient
def generate_augmented_lagrangian_objective(robot_arm, lagrange_multiplier,
mu):
objective_function = generate_objective_function(robot_arm)
constraint_function = generate_constraints_function(robot_arm)
def augmented_lagrangian_objective(thetas):
objective = objective_function(thetas)
constraints = constraint_function(thetas)
return objective - np.sum(
lagrange_multiplier *
constraints) + mu / 2 * np.sum(constraints * constraints)
return augmented_lagrangian_objective
def generate_extended_constraints_function(robot_arm):
constraints_function = generate_constraints_function(robot_arm)
n = robot_arm.n
s = robot_arm.s
def extended_constraints_function(thetas_slack):
thetas = thetas_slack[:n * s]
slack = thetas_slack[n * s:]
assert thetas_slack.shape == (n * s + 2 * n * s, )
constraints = constraints_function(thetas)
additional_constraints = np.zeros(2 * n * s)
additional_constraints[
0::2] = thetas - robot_arm.angular_constraint - slack[0::2]
additional_constraints[
1::2] = robot_arm.angular_constraint - thetas - slack[1::2]
return np.append(constraints, additional_constraints)
return extended_constraints_function
def setUp(self):
self.lengths = (
3,
2,
2,
)
self.destinations = (
(5, 4, 6, 4, 5),
(0, 2, 0.5, -2, -1),
)
self.theta = (
np.pi,
np.pi / 2,
0,
)
self.thetas = np.ones((3 * 5, ))
self.robot_arm = RobotArm(self.lengths, self.destinations, self.theta)
self.constraints_func = generate_constraints_function(self.robot_arm)
self.constraint_gradients_func = generate_constraint_gradients_function(
self.robot_arm)
def generate_quadratically_penalized_objective(robot_arm):
'''
Given a RobotArm object with a valid joint lenghts and destinations
this function returns a quadratically penalized objective function
taking in two parameters: thetas and mu
Function: R^(ns) x R --> R
'''
n = robot_arm.n
s = robot_arm.s
objective = generate_objective_function(robot_arm)
constraints_func = generate_constraints_function(robot_arm)
def quadratically_penalized_objective(thetas, mu):
if not thetas.shape == (n * s,):
raise ValueError('Thetas is not given as 1D-vector, but as: ' + \
str(thetas.shape))
return objective(thetas) + \
0.5 * mu * np.sum(constraints_func(thetas) ** 2)
return quadratically_penalized_objective
def generate_augmented_lagrangian_objective_gradient(robot_arm,
lagrange_multiplier, mu):
s = robot_arm.s
constraints_function = generate_constraints_function(robot_arm)
objective_gradient_function = generate_objective_gradient_function(
robot_arm)
constraint_gradient_function = generate_constraint_gradients_function(
robot_arm)
def augmented_lagrangian_objective_gradient(thetas):
constraints = constraints_function(thetas)
objective_gradient = objective_gradient_function(thetas)
constraint_gradient = constraint_gradient_function(thetas)
second_part = 0
for i in range(2 * s):
second_part += (lagrange_multiplier[i] -
mu * constraints[i]) * constraint_gradient[:, i]
return objective_gradient - second_part
return augmented_lagrangian_objective_gradient
def test_constraint_values2(self):
robot_arm = RobotArm(lengths=(
1,
2,
),
destinations=(
(
1,
-1,
),
(
1,
1,
),
))
thetas = np.array((
0,
np.pi / 2,
np.pi / 2,
np.pi / 4,
))
constraints_func = generate_constraints_function(robot_arm)
constraint_values = constraints_func(thetas)
correct_constraint_values = np.array((
0,
1,
1 - np.sqrt(2),
np.sqrt(2),
))
testing.assert_array_almost_equal(constraint_values,
correct_constraint_values)
constraint_gradients_func = generate_constraint_gradients_function(
robot_arm)
correct = (
(
-2,
1,
0,
0,
),
(
-2,
0,
0,
0,
),
(
0,
0,
-1 - np.sqrt(2),
-np.sqrt(2),
),
(
0,
0,
-np.sqrt(2),
-np.sqrt(2),
),
)
constraint_grads = constraint_gradients_func(thetas)
testing.assert_array_almost_equal(constraint_grads, np.array(correct))
def plot_convergence(xs, objective, robot_arm, show=False):
"""
A function for plotting the convergence of an optimization algorithm.
Input:
xs - The inputs generated iteratively by the optimization method,
given as column vectors in an n times i array, where n is the
dimension of the domain space, and i is the numbers of
iterations performed by the optimization method.
objective - The function to be minimized by the optimization method
"""
# Make sure the xs are never mutated (because I'm a bad programmer)
xs.setflags(write=False)
# Dimension of domain space and number of method iterations
n, i = xs.shape
# The final solution of the method is used as a numerical refernce
# solution
minimizer = xs[:, -1]
minimum = objective(minimizer)
# Calculate remaining distance to final minimizer
remaining_distance = xs - minimizer.reshape((
n,
1,
))
remaining_distance = np.linalg.norm(remaining_distance, ord=2, axis=0)
assert remaining_distance.shape == (i, )
# Calculate the decrement in the objective values
objective_values = np.apply_along_axis(objective, axis=0, arr=xs)
remaining_decline = objective_values - minimum
assert remaining_decline.shape == (i, )
# Calculate the change in constraint values over time
constraints_func = generate_constraints_function(robot_arm)
constraints_values = np.apply_along_axis(constraints_func, axis=0, arr=xs)
print('Constraint_values; ', constraints_values)
constraints_values = np.sum(np.abs(constraints_values), axis=0)
# Create three subplots, one showing convergence of the minimizer,
# the other showing the convergence to the mimimum (input vs. output),
# and the third the values of the constraints over time
plt.style.use('ggplot')
plt.rc('text', usetex=True)
plt.rc('font', family='serif')
# Plot minimizer convergence
ax = plt.subplot('131')
ax.plot(remaining_distance[:-1])
ax.set_yscale('log')
ax.set_title('Convergence towards \n optimal configuration')
ax.set_ylabel(r'$||\Theta - \Theta^*||$')
ax.set_xlabel(r'Iteration number')
# Plot minimum convergence
ax = plt.subplot('132')
ax.plot(objective_values)
ax.set_title('Objective values')
ax.set_ylabel(r'$E(\Theta)$')
ax.set_xlabel(r'Iteration number')
# Plot values of constraints
ax = plt.subplot('133')
ax.plot(constraints_values)
ax.set_title('Equality constraint values')
ax.set_ylabel(r'$\sum_{i=1}^{s}(|c_i,x(\Theta)| + |c_i,y(\Theta)|)$')
ax.set_xlabel(r'Iteration number')
fig = plt.gcf()
fig.set_size_inches(18.5, 5)
plt.savefig('figures/equality_constraint.pdf', bbox_inches='tight')
if show is True:
plt.show()