ipopt and cppad tests

[zrd1234]

When I do the cppad and ipopt joint solution, I run into some problems. The code is as follows.

include

include <cppad/ipopt/solve.hpp>

using namespace std;

namespace { using CppAD::AD; class FG_eval { public: typedef CPPAD_TESTVECTOR(AD) ADvector; void operator()(ADvector& fg, const ADvector& x) { assert(fg.size() == 3); assert(x.size() == 4); // variables AD x1 = x[0]; AD x2 = x[1]; AD x3 = x[2]; AD x4 = x[3]; // f(x) objective function fg[0] = x1 x4 (x1 + x2 + x3) + x3; // constraints fg[1] = x1 x2 x3 x4; fg[2] = x1 x1 + x2 x2 + x3 x3 + x4 * x4; return; }

};

}

bool get_started(void) { bool ok = true; size_t i; typedef CPPAD_TESTVECTOR(double) Dvector;

size_t nx = 4; // number of varibles
size_t ng = 2; // number of constraints
Dvector x0(nx); // initial condition of varibles
x0[0] = 1.0;
x0[1] = 5.0;
x0[2] = 5.0;
x0[3] = 1.0;

// lower and upper bounds for varibles
Dvector xl(nx), xu(nx);
for(i = 0; i < nx; i++)
{
    xl[i] = 1.0;
    xu[i] = 5.0;
}
Dvector gl(ng), gu(ng);
gl[0] = 25.0;    gu[0] = 1.0e19;
gl[1] = 40.0;    gu[1] = 40.0;
// object that computes objective and constraints
FG_eval fg_eval;

// options
string options;
// turn off any printing
options += "Integer print_level  0\n";
options += "String sb            yes\n";
// maximum iterations
options += "Integer max_iter     10\n";
//approximate accuracy in first order necessary conditions;
// see Mathematical Programming, Volume 106, Number 1,
// Pages 25-57, Equation (6)
options += "Numeric tol          1e-6\n";
//derivative tesing
options += "String derivative_test   second-order\n";
// maximum amount of random pertubation; e.g.,
// when evaluation finite diff
options += "Numeric point_perturbation_radius   0.\n";

CppAD::ipopt::solve_result<Dvector> solution; // solution
CppAD::ipopt::solve<Dvector, FG_eval>(options, x0, xl, xu, gl, gu, fg_eval, solution); // solve the problem

cout<<"solution: "<<solution.x<<endl;

//
//check some of the solution values
//
ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
//
double check_x[]  = {1.000000, 4.743000, 3.82115, 1.379408};
double check_zl[] = {1.087871, 0.,       0.,       0.      };
double check_zu[] = {0.,       0.,       0.,       0.      };
double rel_tol    = 1e-6; // relative tolerance
double abs_tol    = 1e-6; // absolute tolerance
for(i = 0; i < nx; i++)
{
    ok &= CppAD::NearEqual(
                check_x[i], solution.x[i], rel_tol, abs_tol);
    ok &= CppAD::NearEqual(
                check_zl[i], solution.zl[i], rel_tol, abs_tol);
    ok &= CppAD::NearEqual(
                check_zu[i], solution.zu[i], rel_tol, abs_tol);
}

return ok;

}

int main() { cout << "CppAD : Hello World Demo!" << endl; get_started(); return 0; } The results are as follows:

[bradbell]

The error message is

error from a known source:
vector: index greater than or equal vector size
...

The best way to debug this is the build a debugging version of your program, run in the debugger, and when the error occurs, go up the stack to find the vector that is smaller than the requested index.

Please try this and report back what you find out.

[zrd1234]

The error message is

error from a known source:
vector: index greater than or equal vector size
...

The best way to debug this is the build a debugging version of your program, run in the debugger, and when the error occurs, go up the stack to find the vector that is smaller than the requested index.

Please try this and report back what you find out.

Hello, I ran it on arm architecture, and found that the problem appeared in the solution step. It was found that the following statement made it impossible to execute. CppAD::ipopt::solve<Dvector, FG_eval>(options, x0, xl, xu, gl, gu, fg_eval, solution); // solve the problem Do you know how to solve it

[bradbell]

I think that if you ran your program in the debugger, you would find that the vector index failure is at the line

    ok &= CppAD::NearEqual( check_x[i], solution.x[i], rel_tol, abs_tol);

because the vector solution.x is empty. If this is so, it is s probably because ipopt returns a failure status value; i.e.,

 solution.status == CppAD::ipopt::solve_result<Dvector>::success;

is false. Check what the value of solution.status is. Perhaps setting print_level to 5 in the line

 options += "Integer print_level  5\n";

will give you more information.

[zrd1234]

options += "Integer print_level 5\n"; Hello, sorry for replying so late. I tried reinstalling ipopt with a reduced version and got good results. But when I run an mpc program, options += "Integer print_level 0\n"; There will be problems. The questions are as follows: When I change it to 5, I don't get an error, but I get some problems.

[bradbell]

Error from an unknown source means that there is an unknown problem on your system. Try running your program using valgrind; https://valgrind.org/docs/manual/quick-start.html

[bradbell]

When I do the cppad and ipopt joint solution, I run into some problems. The code is as follows.

include

include <cppad/ipopt/solve.hpp>

...

The example code above does not compile. When I fix the first include I get the error: /home/bradbell/repo/cppad.git/include/cppad/core/testvector.hpp:93:57: note: expected a type, got ‘AD’

AD must have a template argument; e.g. AD<double> .

[zrd1234]

当我做cppad和ipopt联合解决方案时，我遇到了一些问题。代码如下。 #include #include <cppad/ipopt/solve.hpp> ...

上面的示例代码无法编译。当我修复第一个包含时，我收到错误：/home/bradbell/repo/cppad.git/include/cppad/core/testvector.hpp：93：57：注意：预期一个类型，得到“AD”

AD 必须具有模板参数;例如 .AD<double>

Hello, there is no problem with my program. There is a problem with an mpc program. I wonder if you can compile it.

include "navMpc.h"

//#include <cppad/cppad.hpp>

include <cppad/ipopt/solve.hpp>

include <Eigen/Core>

// The program use fragments of code from // https://github.com/udacity/CarND-MPC-Quizzes

using CppAD::AD;

// ========================================= // FG_eval class definition implementation. // ========================================= class FG_eval { public: // Fitted polynomial coefficients Eigen::VectorXd coeffs;

    double _dt, _ref_cte, _ref_etheta, _ref_vel; 
    double  _w_cte, _w_etheta, _w_vel, _w_angvel, _w_accel, _w_angvel_d, _w_accel_d;
    int _mpc_steps, _x_start, _y_start, _theta_start, _v_start, _cte_start, _etheta_start, _angvel_start, _a_start;

    AD<double> cost_cte, cost_etheta, cost_vel;
    // Constructor
    FG_eval(Eigen::VectorXd coeffs) 
    { 
        this->coeffs = coeffs; 

        // Set default value    
        _dt = 0.1;  // in sec
        _ref_cte   = 0;
        _ref_etheta  = 0;
        _ref_vel   = 0.5; // m/s
        _w_cte     = 100;
        _w_etheta    = 100;
        _w_vel     = 1;
        _w_angvel   = 100;
        _w_accel   = 50;
        _w_angvel_d = 0;
        _w_accel_d = 0;

        _mpc_steps   = 40;
        _x_start     = 0;
        _y_start     = _x_start + _mpc_steps;
        _theta_start   = _y_start + _mpc_steps;
        _v_start     = _theta_start + _mpc_steps;
        _cte_start   = _v_start + _mpc_steps;
        _etheta_start  = _cte_start + _mpc_steps;
        _angvel_start = _etheta_start + _mpc_steps;
        _a_start     = _angvel_start + _mpc_steps - 1;
    }

    // Load parameters for constraints
    void LoadParams(const std::map<string, double> &params)
    {
        _dt = params.find("DT") != params.end() ? params.at("DT") : _dt;
        _mpc_steps = params.find("STEPS") != params.end()    ? params.at("STEPS") : _mpc_steps;
        _ref_cte   = params.find("REF_CTE") != params.end()  ? params.at("REF_CTE") : _ref_cte;
        _ref_etheta  = params.find("REF_ETHETA") != params.end() ? params.at("REF_ETHETA") : _ref_etheta;
        _ref_vel   = params.find("REF_V") != params.end()    ? params.at("REF_V") : _ref_vel;

        _w_cte   = params.find("W_CTE") != params.end()   ? params.at("W_CTE") : _w_cte;
        _w_etheta  = params.find("W_EPSI") != params.end()  ? params.at("W_EPSI") : _w_etheta;
        _w_vel   = params.find("W_V") != params.end()     ? params.at("W_V") : _w_vel;
        _w_angvel = params.find("W_ANGVEL") != params.end() ? params.at("W_ANGVEL") : _w_angvel;
        _w_accel = params.find("W_A") != params.end()     ? params.at("W_A") : _w_accel;
        _w_angvel_d = params.find("W_DANGVEL") != params.end() ? params.at("W_DANGVEL") : _w_angvel_d;
        _w_accel_d = params.find("W_DA") != params.end()     ? params.at("W_DA") : _w_accel_d;

        _x_start     = 0;
        _y_start     = _x_start + _mpc_steps;
        _theta_start   = _y_start + _mpc_steps;
        _v_start     = _theta_start + _mpc_steps;
        _cte_start   = _v_start + _mpc_steps;
        _etheta_start  = _cte_start + _mpc_steps;
        _angvel_start = _etheta_start + _mpc_steps;
        _a_start     = _angvel_start + _mpc_steps - 1;

        //cout << "\n!! FG_eval Obj parameters updated !! " << _mpc_steps << endl; 
    }

    // MPC implementation (cost func & constraints)
    typedef CPPAD_TESTVECTOR(AD<double>) ADvector; 
    // fg: function that evaluates the objective and constraints using the syntax       
    void operator()(ADvector& fg, const ADvector& vars) 
    {
        // fg[0] for cost function
        fg[0] = 0;
        cost_cte =  0;
        cost_etheta = 0;
        cost_vel = 0;

        /*
        for (int i = 0; i < _mpc_steps; i++) 
        {
            cout << i << endl;
            cout << "_x_start" << vars[_x_start + i] <<endl;
            cout << "_y_start" << vars[_y_start + i] <<endl;
            cout << "_theta_start" << vars[_theta_start + i] <<endl;
            cout << "_v_start" << vars[_v_start + i] <<endl;
            cout << "_cte_start" << vars[_cte_start + i] <<endl;
            cout << "_etheta_start" << vars[_etheta_start + i] <<endl;
        }*/

        for (int i = 0; i < _mpc_steps; i++) 
        {
          fg[0] += _w_cte * CppAD::pow(vars[_cte_start + i] - _ref_cte, 2); // cross deviation error
          fg[0] += _w_etheta * CppAD::pow(vars[_etheta_start + i] - _ref_etheta, 2); // heading error
          fg[0] += _w_vel * CppAD::pow(vars[_v_start + i] - _ref_vel, 2); // speed error

          cost_cte +=  _w_cte * CppAD::pow(vars[_cte_start + i] - _ref_cte, 2);
          cost_etheta +=  (_w_etheta * CppAD::pow(vars[_etheta_start + i] - _ref_etheta, 2)); 
          cost_vel +=  (_w_vel * CppAD::pow(vars[_v_start + i] - _ref_vel, 2)); 
        }
        cout << "-----------------------------------------------" <<endl;
        cout << "cost_cte, etheta, velocity: " << cost_cte << ", " << cost_etheta  << ", " << cost_vel << endl;

        // Minimize the use of actuators.
        for (int i = 0; i < _mpc_steps - 1; i++) {
          fg[0] += _w_angvel * CppAD::pow(vars[_angvel_start + i], 2);
          fg[0] += _w_accel * CppAD::pow(vars[_a_start + i], 2);
        }
        cout << "cost of actuators: " << fg[0] << endl; 

        // Minimize the value gap between sequential actuations.
        for (int i = 0; i < _mpc_steps - 2; i++) {
          fg[0] += _w_angvel_d * CppAD::pow(vars[_angvel_start + i + 1] - vars[_angvel_start + i], 2);
          fg[0] += _w_accel_d * CppAD::pow(vars[_a_start + i + 1] - vars[_a_start + i], 2);
        }
        cout << "cost of gap: " << fg[0] << endl; 

        // fg[x] for constraints
        // Initial constraints
        fg[1 + _x_start] = vars[_x_start];
        fg[1 + _y_start] = vars[_y_start];
        fg[1 + _theta_start] = vars[_theta_start];
        fg[1 + _v_start] = vars[_v_start];
        fg[1 + _cte_start] = vars[_cte_start];
        fg[1 + _etheta_start] = vars[_etheta_start];

        // Add system dynamic model constraint
        for (int i = 0; i < _mpc_steps - 1; i++)
        {
            // The state at time t+1 .
            AD<double> x1 = vars[_x_start + i + 1];
            AD<double> y1 = vars[_y_start + i + 1];
            AD<double> theta1 = vars[_theta_start + i + 1];
            AD<double> v1 = vars[_v_start + i + 1];
            AD<double> cte1 = vars[_cte_start + i + 1];
            AD<double> etheta1 = vars[_etheta_start + i + 1];

            // The state at time t.
            AD<double> x0 = vars[_x_start + i];
            AD<double> y0 = vars[_y_start + i];
            AD<double> theta0 = vars[_theta_start + i];
            AD<double> v0 = vars[_v_start + i];
            AD<double> cte0 = vars[_cte_start + i];
            AD<double> etheta0 = vars[_etheta_start + i];

            // Only consider the actuation at time t.
            //AD<double> angvel0 = vars[_angvel_start + i];
            AD<double> w0 = vars[_angvel_start + i];
            AD<double> a0 = vars[_a_start + i];

            //AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * CppAD::pow(x0, 2) + coeffs[3] * CppAD::pow(x0, 3);
            AD<double> f0 = 0.0;
            for (int i = 0; i < coeffs.size(); i++) 
            {
                f0 += coeffs[i] * CppAD::pow(x0, i); //f(0) = y
            }

            //AD<double> trj_grad0 = CppAD::atan(coeffs[1] + 2 * coeffs[2] * x0 + 3 * coeffs[3] * CppAD::pow(x0, 2));
            AD<double> trj_grad0 = 0.0;
            for (int i = 1; i < coeffs.size(); i++) 
            {
                trj_grad0 += i*coeffs[i] * CppAD::pow(x0, i-1); // f'(x0) = f(1)/1
            }
            trj_grad0 = CppAD::atan(trj_grad0);

            // Here's `x` to get you started.
            // The idea here is to constraint this value to be 0.
            //
            // NOTE: The use of `AD<double>` and use of `CppAD`!
            // This is also CppAD can compute derivatives and pass
            // these to the solver.
            // TODO: Setup the rest of the model constraints
            fg[2 + _x_start + i] = x1 - (x0 + v0 * CppAD::cos(theta0) * _dt);
            fg[2 + _y_start + i] = y1 - (y0 + v0 * CppAD::sin(theta0) * _dt);
            fg[2 + _theta_start + i] = theta1 - (theta0 +  w0 * _dt);
            fg[2 + _v_start + i] = v1 - (v0 + a0 * _dt);

            fg[2 + _cte_start + i] = cte1 - ((f0 - y0) + (v0 * CppAD::sin(etheta0) * _dt));
            //fg[2 + _etheta_start + i] = etheta1 - ((theta0 - trj_grad0) + w0 * _dt);//theta0-trj_grad0)->etheta : it can have more curvature prediction, but its gradient can be only adjust positive plan.   
            fg[2 + _etheta_start + i] = etheta1 - (etheta0 + w0 * _dt);
        }
    }

};

// ==================================== // MPC class definition implementation. // ==================================== MPC::MPC() { // Set default value
_mpc_steps = 20; _max_angvel = 3.0; // Maximal angvel radian (~30 deg) _max_throttle = 1.0; // Maximal throttle accel _bound_value = 1.0e3; // Bound value for other variables

_x_start     = 0;
_y_start     = _x_start + _mpc_steps;
_theta_start   = _y_start + _mpc_steps;
_v_start     = _theta_start + _mpc_steps;
_cte_start   = _v_start + _mpc_steps;
_etheta_start  = _cte_start + _mpc_steps;
_angvel_start = _etheta_start + _mpc_steps;
_a_start     = _angvel_start + _mpc_steps - 1;

}

void MPC::LoadParams(const std::map<string, double> &params) { _params = params; //Init parameters for MPC object _mpc_steps = _params.find("STEPS") != _params.end() ? _params.at("STEPS") : _mpc_steps; _max_angvel = _params.find("ANGVEL") != _params.end() ? _params.at("ANGVEL") : _max_angvel; _max_throttle = _params.find("MAXTHR") != _params.end() ? _params.at("MAXTHR") : _max_throttle; _bound_value = _params.find("BOUND") != _params.end() ? _params.at("BOUND") : _bound_value;

_x_start     = 0;
_y_start     = _x_start + _mpc_steps;
_theta_start   = _y_start + _mpc_steps;
_v_start     = _theta_start + _mpc_steps;
_cte_start   = _v_start + _mpc_steps;
_etheta_start  = _cte_start + _mpc_steps;
_angvel_start = _etheta_start + _mpc_steps;
_a_start     = _angvel_start + _mpc_steps - 1;

cout << "\n!! MPC Obj parameters updated !! " << endl; 

}

vector MPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs) { bool ok = true; size_t i; typedef CPPAD_TESTVECTOR(double) Dvector; const double x = state[0]; const double y = state[1]; const double theta = state[2]; const double v = state[3]; const double cte = state[4]; const double etheta = state[5];

// Set the number of model variables (includes both states and inputs).
// For example: If the state is a 4 element vector, the actuators is a 2
// element vector and there are 10 timesteps. The number of variables is:
// 4 * 10 + 2 * 9
size_t n_vars = _mpc_steps * 6 + (_mpc_steps - 1) * 2;

// Set the number of constraints
size_t n_constraints = _mpc_steps * 6;

// Initial value of the independent variables.
// SHOULD BE 0 besides initial state.
Dvector vars(n_vars);
for (int i = 0; i < n_vars; i++) 
{
    vars[i] = 0;
}

// Set the initial variable values
vars[_x_start] = x;
vars[_y_start] = y;
vars[_theta_start] = theta;
vars[_v_start] = v;
vars[_cte_start] = cte;
vars[_etheta_start] = etheta;

// Set lower and upper limits for variables.
Dvector vars_lowerbound(n_vars);
Dvector vars_upperbound(n_vars);

// Set all non-actuators upper and lowerlimits
// to the max negative and positive values.
for (int i = 0; i < _angvel_start; i++) 
{
    vars_lowerbound[i] = -_bound_value;
    vars_upperbound[i] = _bound_value;
}
// The upper and lower limits of angvel are set to -25 and 25
// degrees (values in radians).
for (int i = _angvel_start; i < _a_start; i++) 
{
    vars_lowerbound[i] = -_max_angvel;
    vars_upperbound[i] = _max_angvel;
}
// Acceleration/decceleration upper and lower limits
for (int i = _a_start; i < n_vars; i++)  
{
    vars_lowerbound[i] = -_max_throttle;
    vars_upperbound[i] = _max_throttle;
}

// Lower and upper limits for the constraints
// Should be 0 besides initial state.
Dvector constraints_lowerbound(n_constraints);
Dvector constraints_upperbound(n_constraints);
for (int i = 0; i < n_constraints; i++)
{
    constraints_lowerbound[i] = 0;
    constraints_upperbound[i] = 0;
}
constraints_lowerbound[_x_start] = x;
constraints_lowerbound[_y_start] = y;
constraints_lowerbound[_theta_start] = theta;
constraints_lowerbound[_v_start] = v;
constraints_lowerbound[_cte_start] = cte;
constraints_lowerbound[_etheta_start] = etheta;
constraints_upperbound[_x_start] = x;
constraints_upperbound[_y_start] = y;
constraints_upperbound[_theta_start] = theta;
constraints_upperbound[_v_start] = v;
constraints_upperbound[_cte_start] = cte;
constraints_upperbound[_etheta_start] = etheta;

// object that computes objective and constraints
FG_eval fg_eval(coeffs);
fg_eval.LoadParams(_params);

// options for IPOPT solver
std::string options;
// Uncomment this if you'd like more print information
options += "Integer print_level  0\n";
// NOTE: Setting sparse to true allows the solver to take advantage
// of sparse routines, this makes the computation MUCH FASTER. If you
// can uncomment 1 of these and see if it makes a difference or not but
// if you uncomment both the computation time should go up in orders of
// magnitude.
options += "Sparse  true        forward\n";
options += "Sparse  true        reverse\n";
// NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
// Change this as you see fit.
options += "Numeric max_cpu_time          0.5\n";

// place to return solution
CppAD::ipopt::solve_result<Dvector> solution;

// solve the problem
CppAD::ipopt::solve<Dvector, FG_eval>(
  options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
  constraints_upperbound, fg_eval, solution);

// Check some of the solution values
ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;

// Cost
auto cost = solution.obj_value;
std::cout << "------------ Total Cost(solution): " << cost << "------------" << std::endl;
cout << "max_angvel:" << _max_angvel <<endl;
cout << "max_throttle:" << _max_throttle <<endl;

cout << "-----------------------------------------------" <<endl;

this->mpc_x = {};
this->mpc_y = {};
this->mpc_theta = {};
for (int i = 0; i < _mpc_steps; i++) 
{
    this->mpc_x.push_back(solution.x[_x_start + i]);
    this->mpc_y.push_back(solution.x[_y_start + i]);
    this->mpc_theta.push_back(solution.x[_theta_start + i]);
}

vector<double> result;
result.push_back(solution.x[_angvel_start]);
result.push_back(solution.x[_a_start]);
return result;

}

[bradbell]

It looks like there could be a bug in Mpc, your code, ipopt, or CppAD. CppAD does a lot of error checking (when NDEBUG is not defined) and is detecting the problem. I am not familiar with Mpc. I suggest you try valgrind and see if it detects the problem.

[zrd1234]

看起来 Mpc、您的代码、ipopt 或 CppAD 中可能存在错误。CppAD会进行大量错误检查（未定义NDEBUG时）并检测问题。我不熟悉 Mpc。我建议你试试valgrind，看看它是否能检测到问题。

Thank you for your reply, but I still don't know what the problem is. When I set options += "Integer print_level 0\n"; When the 0 in the code is changed to 5, the result is available, but when it is changed to 1 or 0, the program reports an error.

[bradbell]

Did you try running the program using valgrind ?

You could try to simplify your program. A simpler program that has the same error makes it easier to find. Perhaps you will find a very small change that makes the error go away. This may give you a clue as to where the error is.

[bradbell]

Is this still a problem for you ?