IPOPT не подчиняется ограничениям, но не записывает нарушение при использовании CppAD

Я пытаюсь оценить коэффициенты и время двух полиномов пятого порядка (по одному для положения x и y), которые минимизируют усилие и время (целевую функцию) при соединении начальной позиции, скорости и ориентации с желаемой конечной позицией и ориентация с нулевой скоростью (ограничения равенства). Вот код:

#include <vector>
#include <cppad/cppad.hpp>
#include <cppad/ipopt/solve.hpp>

using CppAD::AD;

typedef struct {
double x, y, theta, linear_velocity;
} Waypoint;

typedef std::vector<Waypoint> WaypointList;

struct TrajectoryConfig {
//! gain on accumulated jerk term in cost function
double Kj;
//! gain on time term in cost function
double Kt;
//! gain on terminal velocity term in cost function
double Kv;
};

class Trajectory {
public:
explicit Trajectory(TrajectoryConfig config);
~Trajectory();
void updateConfigs(TrajectoryConfig config);
void solve(WaypointList waypoints);
private:
//! solution vector
std::vector<double> solution_;
//! gain on accumulated jerk term in cost function
double Kj_;
//! gain on time term in cost function
double Kt_;
//! gain on terminal velocity term in cost function
double Kv_;
};

/*
Trajectory(TrajectoryConfig)

class constructor.  Initializes class given configuration struct
*/
Trajectory::Trajectory(TrajectoryConfig config) {
Kj_ = config.Kj;
Kt_ = config.Kt;
Kv_ = config.Kv;
}

Trajectory::~Trajectory() {
std::cerr << "Trajectory Destructor!" << std::endl;
}

enum Indices { A0 = 0, A1, A2, A3, A4, A5, B0, B1, B2, B3, B4, B5, T };

class FGradEval {
public:
size_t M_;
// gains on cost;
double Kj_, Kt_;
// constructor
FGradEval(double Kj, double Kt) {
M_ = 13;  // no. of parameters per trajectory segment: 2 x 6 coefficients + 1 time
Kj_ = Kj;
Kt_ = Kt;
}

typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
void operator()(ADvector& fgrad, const ADvector& vars) {
fgrad[0] = 0;

AD<double> accum_jerk;
AD<double> a0, a1, a2, a3, a4, a5;
AD<double> b0, b1, b2, b3, b4, b5;
AD<double> T, T2, T3, T4, T5;
AD<double> x, y, vx, vy;

size_t offset = 1;

a0 = vars[Indices::A0];
a1 = vars[Indices::A1];
a2 = vars[Indices::A2];
a3 = vars[Indices::A3];
a4 = vars[Indices::A4];
a5 = vars[Indices::A5];
b0 = vars[Indices::B0];
b1 = vars[Indices::B1];
b2 = vars[Indices::B2];
b3 = vars[Indices::B3];
b4 = vars[Indices::B4];
b5 = vars[Indices::B5];
T  = vars[Indices::T];
T2 = T*T;
T3 = T*T2;
T4 = T*T3;
T5 = T*T4;

x    = a0 + a1*T + a2*T2 + a3*T3 + a4*T4 + a5*T5;
y    = b0 + b1*T + b2*T2 + b3*T3 + b4*T4 + b5*T5;
vx   = a1 + 2*a2*T + 3*a3*T2 + 4*b4*T3 + 5*a5*T4;
vy   = b1 + 2*b2*T + 3*b3*T2 + 4*b4*T3 + 5*b5*T4;

//! cost-terms
//! accum_jerk is the analytic integral of int_0^T (jerk_x^2 + jerk_y^2) dt
accum_jerk = 36 * T * (a3*a3 + b3*b3) + 144 * T2 * (a3*a4 + b3*b4) + T3 * (240*(a3*a5 + b3*b5) + 192*(a4*a4 + b4*b4))
+ 720 * T4 * (a4*a5 + b4*b5) + 720 * T5 * (a5*a5 + b5*b5);
fgrad[0] += Kj_ * accum_jerk;
fgrad[0] += Kt_ * T;

//! initial equality constraints
fgrad[offset]     = vars[Indices::A0];
fgrad[1 + offset] = vars[Indices::B0];
fgrad[2 + offset] = vars[Indices::A1];
fgrad[3 + offset] = vars[Indices::B1];
offset += 4;

//! terminal inequality constraints
fgrad[offset]     = x;
fgrad[offset + 1] = y;
fgrad[offset + 2] = vx;
fgrad[offset + 3] = vy;
}
};

void Trajectory::solve(WaypointList waypoints) {
if (waypoints.size() != 2) {
std::cerr << "Trajectory::solve - Function requires 2 waypoints." << std::endl;
return;
}

//! status flag for solution
bool ok;
//! typedef for ipopt/cppad
typedef CPPAD_TESTVECTOR(double) Dvector;
//! no. of variables for optimization problem
size_t n_vars = 13;
//! no. of constraints
size_t n_cons = 4 * 2;  // the start and final waypoint each contribute 4 constraints (x, y, theta, v) -> (x, y, vx, vy)
//! create vector container for optimizer solution
//! and initialize it to zero
Dvector vars(n_vars);
for (size_t i = 0; i < n_vars; i++) {
vars[i] = 0;
}

//! set initial state (this will only determine the first two coefficients of the initial polynomials)
double v = (fabs(waypoints[0].linear_velocity) < 1e-3)
? 1e-3 : waypoints[0].linear_velocity;
vars[Indices::A0] = waypoints[0].x;
vars[Indices::B0] = waypoints[0].y;
vars[Indices::A1] = v * cos(waypoints[0].theta);
vars[Indices::B1] = v * sin(waypoints[0].theta);
vars[Indices::T] = 0;
//! there are no explicit bounds on vars, so set to something large for the optimizer
//! we could perhaps put bounds on the coeffs corresponding to acc, jerk, snap, ..
Dvector vars_lb(n_vars);
Dvector vars_ub(n_vars);

for (size_t i = 0; i < n_vars; i++) {
vars_lb[i] = -1e10;
vars_ub[i] =  1e10;
}

//! time must be non-negative!
vars_lb[Indices::T] = 0;

//! set the bounds on the constraints
Dvector cons_lb(n_cons);
Dvector cons_ub(n_cons);

//! offset term on index
size_t offset = 0;

//! initial equality constraint - we must start from where we are!
cons_lb[0] = waypoints[0].x;
cons_ub[0] = waypoints[0].x;

cons_lb[1] = waypoints[0].y;
cons_ub[1] = waypoints[0].y;

cons_lb[2] = v * cos(waypoints[0].theta);
cons_ub[2] = v * cos(waypoints[0].theta);

cons_lb[3] = v * sin(waypoints[0].theta);
cons_ub[3] = v * sin(waypoints[0].theta);

offset += 4;

//! terminal point
cons_lb[offset] = waypoints[1].x;
cons_ub[offset] = waypoints[1].x;

cons_lb[offset + 1] = waypoints[1].y;
cons_ub[offset + 1] = waypoints[1].y;

cons_lb[offset + 2] = 1e-3 * cos(waypoints[1].theta);
cons_ub[offset + 2] = 1e-3 * cos(waypoints[1].theta);

cons_lb[offset + 3] = 1e-3 * sin(waypoints[1].theta);
cons_ub[offset + 3] = 1e-3 * sin(waypoints[1].theta);

//! create instance of objective function class
FGradEval fg_eval(Kj_, Kt_);

//! IPOPT INITIALIZATION
std::string options;
options += "Integer print_level  5\n";
options += "Sparse  true        forward\n";
options += "Sparse  true        reverse\n";
options += "Integer max_iter         100\n";
// options += "Numeric tol         1e-4\n";

//! compute the solution
CppAD::ipopt::solve_result<Dvector> solution;

//! solve
CppAD::ipopt::solve<Dvector, FGradEval>(
options, vars, vars_lb, vars_ub, cons_lb, cons_ub, fg_eval, solution);

//! check if the solver was successful
ok = solution.status == CppAD::ipopt::solve_result<Dvector>::success;

//! if the solver was unsuccessful, exit
//! this case will be handled by calling method
if (!ok) {
std::cerr << "Trajectory::solve - Failed to find a solution!" << std::endl;
return;
}

//! (DEBUG) output the final cost
std::cout << "Final Cost: " << solution.obj_value << std::endl;

//! populate output with argmin vector
for (size_t i = 0; i < n_vars; i++) {
solution_.push_back(solution.x[i]);
}

return;
}

Где у меня возникли проблемы в следующем:

1. Начальное ограничение равенства (начальная позиция, скорость и ориентация) поддерживается, а конечное ограничение скорости — нет. Алгоритм заканчивается в правильном финале (x, y, угол), но скорость не равна нулю. Я просмотрел код и не могу понять, почему положение и ориентация в конечной точке будут соблюдаться, а скорость — нет. Я подозреваю, что мое определение ограничений равенства не то, что я думаю.
2. Проблема не сходится регулярно, но это, как определено, довольно простая проблема (см. Вывод)

******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
Ipopt is released as open source code under the Eclipse Public License (EPL).
For more information visit http://projects.coin-or.org/Ipopt
******************************************************************************

This is Ipopt version 3.11.9, running with linear solver mumps.
NOTE: Other linear solvers might be more efficient (see Ipopt documentation).

Number of nonzeros in equality constraint Jacobian...:       30
Number of nonzeros in inequality constraint Jacobian.:        0
Number of nonzeros in Lagrangian Hessian.............:       23

Total number of variables............................:       13
variables with only lower bounds:        0
variables with lower and upper bounds:       13
variables with only upper bounds:        0
Total number of equality constraints.................:        8
Total number of inequality constraints...............:        0
inequality constraints with only lower bounds:        0
inequality constraints with lower and upper bounds:        0
inequality constraints with only upper bounds:        0

iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
0  9.9999900e-03 1.00e+00 5.00e-04  -1.0 0.00e+00    -  0.00e+00 0.00e+00   0
1  5.9117705e-02 1.00e+00 1.20e+02  -1.0 5.36e+07    -  1.04e-05 7.63e-06f 18
2  1.1927070e+00 1.00e+00 2.62e+06  -1.0 9.21e+05  -4.0 6.16e-15 2.29e-23H  1
3  2.9689692e-01 1.00e+00 1.80e+05  -1.0 2.24e+13    -  1.83e-07 8.42e-10f 20
4r 2.9689692e-01 1.00e+00 1.00e+03  -0.0 0.00e+00    -  0.00e+00 4.58e-07R 11
5r 2.1005820e+01 9.99e-01 5.04e+02  -0.0 6.60e-02    -  9.90e-01 4.95e-01f  2
6r 7.7118141e+04 9.08e-01 5.18e+03  -0.0 2.09e+00    -  4.21e-01 1.00e+00f  1
7r 1.7923891e+04 7.82e-01 1.54e+03  -0.0 3.63e+00    -  9.90e-01 1.00e+00f  1
8r 5.9690221e+03 5.41e-01 5.12e+02  -0.0 2.92e+00    -  9.90e-01 1.00e+00f  1
9r 4.6855625e+03 5.54e-01 1.95e+02  -0.0 5.14e-01    -  9.92e-01 1.00e+00f  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
10r 8.4901226e+03 5.55e-01 5.18e+01  -0.0 2.24e-01    -  1.00e+00 1.00e+00f  1

Number of Iterations....: 10

(scaled)                 (unscaled)
Objective...............:   8.4901225582208808e+03    8.4901225582208808e+03
Dual infeasibility......:   6.3613117039244315e+06    6.3613117039244315e+06
Constraint violation....:   5.5503677023620179e-01    5.5503677023620179e-01
Complementarity.........:   9.9999982900301554e-01    9.9999982900301554e-01
Overall NLP error.......:   6.3613117039244315e+06    6.3613117039244315e+06Number of objective function evaluations             = 43
Number of objective gradient evaluations             = 6
Number of equality constraint evaluations            = 71
Number of inequality constraint evaluations          = 0
Number of equality constraint Jacobian evaluations   = 12
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations             = 10
Total CPU secs in IPOPT (w/o function evaluations)   =      0.006
Total CPU secs in NLP function evaluations           =      0.001

EXIT: Maximum Number of Iterations Exceeded.

Я не ищу ответ на мою проблему специально. На что я надеюсь, так это на некоторые предположения о том, почему моя проблема может работать не так, как ожидалось. В частности, имеют ли смысл мои ограничения, как они определены? Правильно ли выполнена инициализация переменной?