ALM converging but not satisfying constraints

[ruairimoran]

Describe the bug

F1 constraints must belong to the set c = (-\infty, 0]. The solver returns Converged, but the solution does not satisfy (all of) the constraints.

To Reproduce

Steps to reproduce the behavior:

1. put create_solver.py and run_solver.py in a folder
2. change open local path in create_solver.py
3. run create_solver.py
4. run run_solver.py
5. note that the solver should not converge because two of the shapes overlap, but it does return Converged
6. note the printed variable x in run_solver.py: this should belong to the set c (but it will be positive)

Expected behavior

If the vehicle shape overlaps with either obstacle shape, solver should not converge. Otherwise, it should converge.

System information:

• System/Platform [Laptop]

• OS: [Windows]

• What is the output of rustup show?

Default host: x86_64-pc-windows-msvc rustup home: C:\Users\40232020.rustup

installed toolchains stable-x86_64-pc-windows-msvc (default) nightly-x86_64-pc-windows-msvc

active toolchain stable-x86_64-pc-windows-msvc (default) rustc 1.65.0 (897e37553 2022-11-02)

• What is the output of rustc -V?

rustc 1.65.0 (897e37553 2022-11-02)

• Python 3.10

Additional context

The solver always works as expected with the first obstacle you pass in, but doesn't work for any more passed in

create_solver.py

import numpy as np
import casadi.casadi as cs
import opengen as og
import logging as lg

num_obstacles = 2

if __name__ == "__main__":
    epsilon = cs.SX.sym('epsilon', 1)
    squircle_q = cs.SX.sym('squircle_q', 1)
    vehicle = cs.SX.sym('vehicle', 3)
    obstacles = cs.SX.sym('obstacles', 3 * num_obstacles)
    hyperplanes = cs.SX.sym('hyperplanes', 2 * num_obstacles)

    # vehicle params
    vehicle_pos = vehicle[0:2]
    vehicle_radius = vehicle[2]

    # obstacles params
    obstacle_pos, obstacle_radius = [0] * num_obstacles, [0] * num_obstacles
    for i in range(num_obstacles):
        index = i * 3
        obstacle_pos[i] = obstacles[index: index + 2]
        obstacle_radius[i] = obstacles[index + 2]

    # hyperplanes params
    hyperplane_pos = [0] * num_obstacles
    for i in range(num_obstacles):
        index = i * 2
        hyperplane_pos[i] = hyperplanes[index: index + 2]

    # cost
    cost = 1

    # alm set c
    set_c = og.constraints.Rectangle([-np.infty], [0])

    # alm constraints memory
    hyp_0_to_power_of_q, hyp_1_to_power_of_q = [0] * num_obstacles, [0] * num_obstacles
    q_norm_h, radii, pos_diff = [0] * num_obstacles, [0] * num_obstacles, [0] * num_obstacles

    # alm constraint 1 -- obstacle 1 with hyperplane 1
    hyp_0_to_power_of_q[0] = cs.power(cs.fabs(hyperplane_pos[0][0]), squircle_q)
    hyp_1_to_power_of_q[0] = cs.power(cs.fabs(hyperplane_pos[0][1]), squircle_q)
    q_norm_h[0] = cs.power(hyp_0_to_power_of_q[0] + hyp_1_to_power_of_q[0], 1 / squircle_q)
    radii[0] = vehicle_radius + obstacle_radius[0]
    pos_diff[0] = vehicle_pos - obstacle_pos[0]
    overlap_penalty_0 = radii[0] * q_norm_h[0] + cs.dot(hyperplane_pos[0], pos_diff[0]) + epsilon

    # alm constraint 2 -- obstacle 2 with hyperplane 2
    hyp_0_to_power_of_q[1] = cs.power(cs.fabs(hyperplane_pos[1][0]), squircle_q)
    hyp_1_to_power_of_q[1] = cs.power(cs.fabs(hyperplane_pos[1][1]), squircle_q)
    q_norm_h[1] = cs.power(hyp_0_to_power_of_q[1] + hyp_1_to_power_of_q[1], 1 / squircle_q)
    radii[1] = vehicle_radius + obstacle_radius[1]
    pos_diff[1] = vehicle_pos - obstacle_pos[1]
    overlap_penalty_1 = radii[1] * q_norm_h[1] + cs.dot(hyperplane_pos[1], pos_diff[1]) + epsilon

    # alm constraints F1
    overlap_penalty = cs.vertcat(overlap_penalty_0, overlap_penalty_1)

    # bounds
    bounds_index_list = [2 * count + 1 for count in range(num_obstacles)]
    balls = [og.constraints.Ball2(None, 1)] * num_obstacles
    bounds = og.constraints.CartesianProduct(bounds_index_list, balls)

    # pass problem to optimizer
    param = cs.vertcat(epsilon, squircle_q, vehicle, obstacles)
    problem = og.builder.Problem(hyperplanes, param, cost) \
        .with_constraints(bounds) \
        .with_aug_lagrangian_constraints(overlap_penalty, set_c)
    build_config = og.config.BuildConfiguration() \
        .with_open_version(local_path="../optimization-engine") \
        .with_build_directory("optimizers") \
        .with_build_mode("debug") \
        .with_build_python_bindings()
    meta = og.config.OptimizerMeta() \
        .with_version("1.0.0") \
        .with_authors(["Ruairi Moran"]) \
        .with_optimizer_name("hyperplane")
    solver_config = og.config.SolverConfiguration() \
        .with_max_outer_iterations(20) \
        .with_tolerance(1e-2) \
        .with_initial_tolerance(1e-2) \
        .with_delta_tolerance(1e-2) \
        .with_initial_penalty(5000.0) \
        .with_penalty_weight_update_factor(1.1) \
        .with_preconditioning(True)
    builder = og.builder.OpEnOptimizerBuilder(problem,
                                              meta,
                                              build_config,
                                              solver_config) \
        .with_verbosity_level(lg.INFO)
    builder.build()

    print("***  Success! Vehicle controller created. *** ")

run_solver.py

from create_solver import num_obstacles
import numpy as np
import matplotlib.pyplot as plt
import sys
sys.path.insert(1, 'optimizers/hyperplane')
import hyperplane
solver = hyperplane.solver()

# squircles setup
squircle_p = 10  # must be greater than 1
squircle_q = squircle_p / (squircle_p - 1)

# simulation params
v = 0
o1, o2, o3, o4 = 1.4, 1, 1, 1
vehicle = [-v, v, 1]  # x, y, radius
obstacles = [[-o1, -o1, .4],
             [o2, o2, .2]]

# hyperplane params
epsilon = 1e-1

# run solver
params = np.hstack((epsilon, squircle_q, vehicle, np.hstack(obstacles)))
rand_nonzero_vec = np.random.randn(2 * num_obstacles)
hyp = solver.run(p=params, initial_guess=rand_nonzero_vec)
print(f"exit status: {hyp.exit_status},\n"
      f"solve time: {hyp.solve_time_ms},\n"
      f"penalty param: {hyp.penalty},\n"
      f"outer iters: {hyp.num_outer_iterations}\n"
      f"F1 infeasibility: {hyp.f1_infeasibility}")

u_star = hyp.solution
print(f"u_star = {u_star}")

h2 = u_star[2:4]
radii = vehicle[2] + obstacles[1][2]
pos_diff_0 = vehicle[0] - obstacles[1][0]
pos_diff_1 = vehicle[1] - obstacles[1][1]
hyp_0_to_power_of_q = np.power(np.abs(h2[0]), squircle_q)
hyp_1_to_power_of_q = np.power(np.abs(h2[1]), squircle_q)
q_norm_h = np.power(hyp_0_to_power_of_q + hyp_1_to_power_of_q, 1 / squircle_q)
x = radii * q_norm_h + h2[0] * pos_diff_0 + h2[1] * pos_diff_1 + epsilon
print(x)  # should be nonpositive

def draw_line(coord_x_, coord_y_, angle_in_rad_):
    length_ = .01  # arrow length
    plt.arrow(coord_x_, coord_y_, length_ * np.cos(angle_in_rad_), length_ * np.sin(angle_in_rad_), head_width=0.2)

def draw_plane(coord_x_, coord_y_):
    mul = 1 / (max(abs(coord_x_), abs(coord_y_)) * 1e1)
    plt.quiver(*np.array([[0], [0]]), coord_x_*mul, coord_y_*mul, scale=1)

def draw_squircle(coord_x_, coord_y_, radius_):
    step_ = 0.05  # step length between points
    plot_number_ = np.arange(0, 2*np.pi+step_, step_)  # points
    # scaling parameters
    scaling_x = 1  # scaling on east axis
    scaling_y = 1  # scaling on north axis
    # create circle points
    x_ = coord_x_ + (radius_ * np.sign(np.cos(plot_number_)) *
                     abs(np.cos(plot_number_)) ** (2 / squircle_p) * scaling_x)
    y_ = coord_y_ + (radius_ * np.sign(np.sin(plot_number_)) *
                     abs(np.sin(plot_number_)) ** (2 / squircle_p) * scaling_y)
    # rotation parameters
    theta_ = 0  # rotation angle in radians
    # rotate circle points
    rotated_x_ = -np.sin(theta_) * y_ + np.cos(theta_) * x_
    rotated_y_ = np.cos(theta_) * y_ + np.sin(theta_) * x_
    # plot points
    for i_ in range(len(plot_number_)):
        plt.plot(rotated_x_, rotated_y_)

# plot squircles
fig3 = plt.figure()
axs3 = fig3.add_subplot(111)
plt.xlabel('x (m)')
plt.ylabel('y (m)')
axs3.set_aspect('equal', adjustable='box')
draw_squircle(vehicle[0], vehicle[1], vehicle[2])
for i in range(num_obstacles):
    index = i * 2
    draw_squircle(obstacles[i][0], obstacles[i][1], obstacles[i][2])
    draw_plane(hyp.solution[index], hyp.solution[index + 1])
    print(f"hyp_{i} = [{hyp.solution[index]}, {hyp.solution[index + 1]}]")

plt.savefig('sep_hyp.jpg', format='jpg', dpi=1200)
plt.show()

[alphaville]

@smokinmirror I noticed that this line does not perform the projection correctly. It only projects the first coordinate of y. I suppose alm_set_c has the wrong dimension.

[alphaville]

@smokinmirror I found the culprint: in the auto-generated optimiser, in src/lib.rs, the set C is defined using

const SET_C_XMIN :Option<&[f64]> = Some(&[std::f64::NEG_INFINITY,]);
const SET_C_XMAX :Option<&[f64]> = Some(&[0.0,]);

The dimension of these vectors should be 2, not 1.

[alphaville]

@smokinmirror you just need to change your definition of set_c to

set_c = og.constraints.Rectangle([-np.infty, -np.infty], [0, 0])

and the problem will be solved. I mean... not solved, since this is an infeasible problem.