C++ SLSQP Not Giving Updated Variables After Completing Optimization

[nocholasrift]

I'm using the SLSQP algorithm for an implementation of Model Predictive Control. I've setup the constraints, objective, and the gradients of each to the optimizer. When it runs, the return code is non-negative. I initialize all the values to 0 right now as an initial guess, but when the optimizer finished (and returns a non-negative error code), the results stay the same. Also, the cost reverts back to the initial value as well.

The gradients could be also be wrong based on the cost signal from my terminal output. However, the optimizer still seems to be finding a better solution that the initial guess, and I would assume it would load that into the variable vector.

I also thought this could be a constraint violation issue, as the constraints are violating the set tolerances. However, if it is such a problem, wouldn't the optimizer report that back to the user through an exception/print-statement/return code? Thanks in advance for any help!

Code: ` std::vector MPCNLOPT::Solve_gtg(const Eigen::VectorXd &state) { size_t n_vars = _mpc_steps * state.size() + (_mpc_steps - 1) * 2; size_t n_constraints = (_mpc_steps - 1) * state.size();

double x = state(0);
double y = state(1);
double theta = state(2);
double v = state(3);

_state = state;

// Requires gradients
nlopt::opt opt(nlopt::algorithm::LD_SLSQP, n_vars);

std::vector<double> vars_lower_bound(n_vars);
std::vector<double> vars_upper_bound(n_vars);

for (int i = 0; i < _angvel_start; ++i)
{
    vars_lower_bound[i] = -HUGE_VAL;
    vars_upper_bound[i] = HUGE_VAL;
}

for (int i = _v_start; i < _v_start + _mpc_steps; ++i)
{
    vars_lower_bound[i] = 0;
    vars_upper_bound[i] = _max_linvel;
}

for (int i = _angvel_start; i < _linacc_start; ++i)
{
    vars_lower_bound[i] = -_max_angvel;
    vars_upper_bound[i] = _max_angvel;
}

for (int i = _linacc_start; i < n_vars; ++i)
{
    vars_lower_bound[i] = -2 * _max_linvel;
    vars_upper_bound[i] = 2 * _max_linvel;
}

opt.set_lower_bounds(vars_lower_bound);
opt.set_upper_bounds(vars_upper_bound);

opt.set_min_objective(MPCNLOPT::objective_gtg, this);

std::vector<double> tolerances(n_constraints, 1e-6);
opt.add_equality_mconstraint(MPCNLOPT::dynamics_constraints_gtg, this, tolerances);
opt.set_ftol_rel(1e-4);
opt.set_xtol_rel(1e-6);
opt.set_maxtime(.5);

std::vector<double> xi(n_vars, 0);
xi[_x_start] = x;
xi[_y_start] = y;
xi[_theta_start] = theta;
xi[_v_start] = v;

double minf;

try
{
    nlopt::result result = opt.optimize(xi, minf);
    std::cout << "found minimum at f(x)= "
              << std::setprecision(10) << minf << std::endl;

    std::cout << "result is " << result << std::endl;
}
catch (std::exception &e)
{
    std::cerr << "nlopt failed: " << e.what() << std::endl;
}

mpc_x = {};
mpc_y = {};
mpc_theta = {};
mpc_linvels = {};

for (int i = 0; i < _mpc_steps; ++i)
{
    mpc_x.push_back(xi[_x_start + i]);
    mpc_y.push_back(xi[_y_start + i]);
    mpc_theta.push_back(xi[_theta_start + i]);
    mpc_linvels.push_back(xi[_v_start + i]);
}

mpc_angvels = {};
mpc_linaccs = {};

for (int i = 0; i < _mpc_steps - 1; ++i)
{
    mpc_angvels.push_back(xi[_angvel_start + i]);
    mpc_linaccs.push_back(xi[_linacc_start + i]);
}

// print positions in horizon in readable format
std::cout << "Horizon positions: " << std::endl;
for (int i = 0; i < _mpc_steps; ++i)
{
    std::cout << "x: " << mpc_x[i] << ", y: " << mpc_y[i] << ", theta: " << mpc_theta[i] << ", v: " << mpc_linvels[i] << std::endl;
}

std::vector<double> ret;
ret.push_back(xi[_angvel_start]);
ret.push_back(xi[_linacc_start]);

return ret;

} `

Terminal Output:

defining state starting solve gradient size is 118 COST IS 1920 gradient size is 118 COST IS 45222.9 gradient size is 0 COST IS 2.7297 gradient size is 118 COST IS 2.7297 gradient size is 118 COST IS 65.3584 gradient size is 0 COST IS 3.98063 gradient size is 0 COST IS 2.25721 gradient size is 0 COST IS 2.33999 gradient size is 0 COST IS 2.49847 gradient size is 0 COST IS 2.60219 gradient size is 0 COST IS 2.66119 gradient size is 0 COST IS 2.69329 gradient size is 0 COST IS 2.71045 gradient size is 0 COST IS 2.71955 gradient size is 0 COST IS 2.72436 gradient size is 118 COST IS 2.72436 gradient size is 118 COST IS 38.9143 gradient size is 0 COST IS 4.68046 gradient size is 0 COST IS 3.02822 gradient size is 0 COST IS 2.79191 gradient size is 0 COST IS 2.74342 gradient size is 0 COST IS 2.73084 gradient size is 0 COST IS 2.72687 gradient size is 0 COST IS 2.72541 gradient size is 0 COST IS 2.72482 gradient size is 0 COST IS 2.72456 gradient size is 0 COST IS 2.72445 gradient size is 118 COST IS 2.72445 found minimum at f(x)= 1920 result is 3 Horizon values: x: -3.147294288e-14, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0 x: 0, y: 0, theta: 0, v: 0

[Yuanzidan081]

Hi, I also encountered this problem. Have you found a method to fix it? It seems that the final solution doesn't update and remains the same as the initial value.

[nocholasrift]

Hi there. My issue ended up being that the gradients of my constraints and objective function were not correct, as there was a bug in my auto-differentiation code. Once I fixed that it behave correctly.