ipopt and cppad tests about cppad

[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.