The result of function CppAD::ipopt::solve<Dvector, FG_eval>() is null.

[Tengyun-Mo]

The result of function CppAD::ipopt::solve<Dvector, FG_eval>( options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,constraints_upperbound, fg_eval, solution) is null.And I don't know what cause this.Does anyone knows what can cause the result is null?I'm very appreciate it .

fg :{ 16000, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2.66548, 2.66548, 2.66548, 2.66548, 2.66548, 2.66548, 2.66548, 2.66548, 2.66548, 0, -0.0842298, -0.0842298, -0.0842298, -0.0842298, -0.0842298, -0.0842298, -0.0842298, -0.0842298, -0.0842298 }
solution.x:
{  }
Cost 6.42285e-323

This is the code:

#include "MPC.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 _Lf, _dt, _ref_cte, _ref_epsi, _ref_vel; 
        double  _w_cte, _w_epsi, _w_vel, _w_delta, _w_accel, _w_delta_d, _w_accel_d;
        int _mpc_steps, _x_start, _y_start, _psi_start, _v_start, _cte_start, _epsi_start, _delta_start, _a_start;

        // Constructor
        FG_eval(Eigen::VectorXd coeffs) 
        { 
            this->coeffs = coeffs; 

            // Set default value    
            _Lf = 1.25; // distance between the front of the vehicle and its center of gravity
            _dt = 0.05;  // in sec
            _ref_cte   = 0;
            _ref_epsi  = 0;
            _ref_vel   = 4.0; // m/s
            _w_cte     = 100;
            _w_epsi    = 100;
            _w_vel     = 100;
            _w_delta   = 100;
            _w_accel   = 50;
            _w_delta_d = 0;
            _w_accel_d = 0;

            _mpc_steps   = 10;
            _x_start     = 0;
            _y_start     = _x_start + _mpc_steps;
            _psi_start   = _y_start + _mpc_steps;
            _v_start     = _psi_start + _mpc_steps;
            _cte_start   = _v_start + _mpc_steps;
            _epsi_start  = _cte_start + _mpc_steps;
            _delta_start = _epsi_start + _mpc_steps;
            _a_start     = _delta_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;
            _Lf = params.find("LF") != params.end() ? params.at("LF") : _Lf;
            _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_epsi  = params.find("REF_EPSI") != params.end() ? params.at("REF_EPSI") : _ref_epsi;
            _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_epsi  = params.find("W_EPSI") != params.end()  ? params.at("W_EPSI") : _w_epsi;
            _w_vel   = params.find("W_V") != params.end()     ? params.at("W_V") : _w_vel;
            _w_delta = params.find("W_DELTA") != params.end() ? params.at("W_DELTA") : _w_delta;
            _w_accel = params.find("W_A") != params.end()     ? params.at("W_A") : _w_accel;
            _w_delta_d = params.find("W_DDELTA") != params.end() ? params.at("W_DDELTA") : _w_delta_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;
            _psi_start   = _y_start + _mpc_steps;
            _v_start     = _psi_start + _mpc_steps;
            _cte_start   = _v_start + _mpc_steps;
            _epsi_start  = _cte_start + _mpc_steps;
            _delta_start = _epsi_start + _mpc_steps;
            _a_start     = _delta_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) 
        {
            cout<<"calculate mpc trajectory"<<endl;
            cout<<"fg vars:"<<vars<<endl;
            // fg[0] for cost function
            fg[0] = 0;
            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_epsi * CppAD::pow(vars[_epsi_start + i] - _ref_epsi, 2); // heading error
              fg[0] += _w_vel * CppAD::pow(vars[_v_start + i] - _ref_vel, 2); // speed error
            }

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

            // Minimize the value gap between sequential actuations.
            for (int i = 0; i < _mpc_steps - 2; i++) {
              fg[0] += _w_delta_d * CppAD::pow(vars[_delta_start + i + 1] - vars[_delta_start + i], 2);
              fg[0] += _w_accel_d * CppAD::pow(vars[_a_start + i + 1] - vars[_a_start + i], 2);
            }
            cout<<"fg0 :"<<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 + _psi_start] = vars[_psi_start];
            fg[1 + _v_start] = vars[_v_start];
            fg[1 + _cte_start] = vars[_cte_start];
            fg[1 + _epsi_start] = vars[_epsi_start];

            // Add system dynamic model constraint
            // The rest of the constraints
            for (int t = 1; t < _mpc_steps; t++) {
                // State at time t + 1
                AD<double> x1 = vars[_x_start + t];
                AD<double> y1 = vars[_y_start + t];
                AD<double> psi1 = vars[_psi_start + t];
                AD<double> v1 = vars[_v_start + t];
                AD<double> cte1 = vars[_cte_start + t];
                AD<double> epsi1 = vars[_epsi_start + t];

                // State at time t
                AD<double> x0 = vars[_x_start + t - 1];
                AD<double> y0 = vars[_y_start + t - 1];
                AD<double> psi0 = vars[_psi_start + t - 1];
                AD<double> v0 = vars[_v_start + t - 1];
                AD<double> cte0 = vars[_cte_start + t - 1];
                AD<double> epsi0 = vars[_epsi_start + t - 1];

                // Actuator constraints at time t only
                AD<double> delta0 = vars[_delta_start + t - 1];
                AD<double> a0 = vars[_a_start + t - 1];

                AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * pow(x0, 2) + coeffs[3] * pow(x0, 3);
                AD<double> psi_des0 = CppAD::atan(coeffs[1] + 2*coeffs[2]*x0 + 3*coeffs[3]*pow(x0,2));

                // Setting up the rest of the model constraints
                fg[1 + _x_start + t] = x1 - (x0 + v0 * CppAD::cos(psi0) * _dt);
                fg[1 + _y_start + t] = y1 - (y0 + v0 * CppAD::sin(psi0) * _dt);
                fg[1 + _psi_start + t] = psi1 - (psi0 - v0 * delta0 / _Lf * _dt);
                fg[1 + _v_start + t] = v1 - (v0 + a0 * _dt);
                fg[1 + _cte_start + t] = cte1 - ((f0-y0) + (v0 * CppAD::sin(epsi0) * _dt));
                fg[1 + _epsi_start + t] = epsi1 - ((psi0 - psi_des0) - v0 * delta0 / _Lf * _dt);
            }
            cout<<"fg :"<<fg<<endl;
        }
};

// ====================================
// MPC class definition implementation.
// ====================================
MPC::MPC() 
{
    // Set default value    
    _mpc_steps = 10;
    _max_steering = 0.523; // Maximal steering 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;
    _psi_start   = _y_start + _mpc_steps;
    _v_start     = _psi_start + _mpc_steps;
    _cte_start   = _v_start + _mpc_steps;
    _epsi_start  = _cte_start + _mpc_steps;
    _delta_start = _epsi_start + _mpc_steps;
    _a_start     = _delta_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_steering = _params.find("MAXSTR") != _params.end() ? _params.at("MAXSTR") : _max_steering;
    _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;
    _psi_start   = _y_start + _mpc_steps;
    _v_start     = _psi_start + _mpc_steps;
    _cte_start   = _v_start + _mpc_steps;
    _epsi_start  = _cte_start + _mpc_steps;
    _delta_start = _epsi_start + _mpc_steps;
    _a_start     = _delta_start + _mpc_steps - 1;

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

vector<double> 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 psi = state[2];
    const double v = state[3];
    const double cte = state[4];
    const double epsi = 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:
    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;
    }

    Dvector vars_lowerbound(n_vars);
    Dvector vars_upperbound(n_vars);
    // Set lower and upper limits for variables.
    for (int i = 0; i < _delta_start; i++) 
    {
        vars_lowerbound[i] = -_bound_value;
        vars_upperbound[i] = _bound_value;
    }
    // The upper and lower limits of delta are set to -25 and 25
    // degrees (values in radians).
    for (int i = _delta_start; i < _a_start; i++) 
    {
        vars_lowerbound[i] = -_max_steering;
        vars_upperbound[i] = _max_steering;
    }
    // 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[_psi_start] = psi;
    constraints_lowerbound[_v_start] = v;
    constraints_lowerbound[_cte_start] = cte;
    constraints_lowerbound[_epsi_start] = epsi;
    constraints_upperbound[_x_start] = x;
    constraints_upperbound[_y_start] = y;
    constraints_upperbound[_psi_start] = psi;
    constraints_upperbound[_v_start] = v;
    constraints_upperbound[_cte_start] = cte;
    constraints_upperbound[_epsi_start] = epsi;

    // 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;
    cout<<"vars :\n"<<vars<<endl;
    cout<<"vars_lowerbound :\n"<<vars_lowerbound<<endl;
    cout<<"vars_upperbound :\n"<<vars_upperbound<<endl;
    // solve the problem
    CppAD::ipopt::solve<Dvector, FG_eval>(
      options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
      constraints_upperbound, fg_eval, solution);
    cout<<"solution:\n"<<solution.x<<endl;
    // Check some of the solution values
    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
    //cout<<"test5"<<endl;
    // Cost
    auto cost = solution.obj_value;
    std::cout << "Cost " << cost << std::endl;
    this->mpc_x = {};

    for (int i = 0; i < _mpc_steps; i++) 
    {
        cout<<"test6"<<endl;
        this->mpc_x.push_back(solution.x[_x_start + i]);
        cout<<"test7"<<endl;
        this->mpc_y.push_back(solution.x[_y_start + i]);
    }
    // Return the first actuator values, along with predicted x and y values to plot in the simulator.
//    std::vector<double> solved;
//    solved.push_back(solution.x[delta_start]);
//    solved.push_back(solution.x[a_start]);
//    for (int i = 0; i < N; ++i) {
//        solved.push_back(solution.x[x_start + i]);
//        solved.push_back(solution.x[y_start + i]);
//    }
    vector<double> result;
    result.push_back(solution.x[_delta_start]);
    result.push_back(solution.x[_a_start]);
    return result;
}

[bradbell]

I am using a fedora system. I tried installing mpc and have it, but I cannot find MPC.h and Fedora does not seem to have a development version of mpc; i.e.,

cppad_issue>sudo dnf install mpc-devel
Last metadata expiration check: 0:04:44 ago on Fri 09 Oct 2020 10:13:22 AM MST.
No match for argument: mpc-devel
Error: Unable to find a match: mpc-devel

Can you remove mpc from the example ?

[Tengyun-Mo]

I am using a fedora system. I tried installing mpc and have it, but I cannot find MPC.h and Fedora does not seem to have a development version of mpc; i.e.,

cppad_issue>sudo dnf install mpc-devel
Last metadata expiration check: 0:04:44 ago on Fri 09 Oct 2020 10:13:22 AM MST.
No match for argument: mpc-devel
Error: Unable to find a match: mpc-devel

Can you remove mpc from the example ?

Thanks for you response,I try the demo of Ipopt,and everything is ok.I think maybe something wrong with input vector.there is the mpc.h:

#ifndef MPC_H
#define MPC_H

#include <vector>
#include <map>
#include "Eigen-3.3/Eigen/Core"

using namespace std;

class MPC {
 public:
    MPC();
    vector<double> mpc_x;
    vector<double> mpc_y;

    void LoadParams(const std::map<string, double> &params);
    // Solve the model given an initial state and polynomial coefficients.
    // Return the first actuations.
    vector<double> Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs);
    virtual ~MPC();

 private:
    // Parameters for mpc solver
    double _max_steering, _max_throttle, _bound_value;
    int _mpc_steps, _x_start, _y_start, _psi_start, _v_start, _cte_start, _epsi_start, _delta_start, _a_start;
    std::map<string, double> _params;

};

#endif /* MPC_H */

[bradbell]

I put your MPC.h at the front inline with at the top of the rest of your program in a file called junk.cpp and tried to compile it. I got the following errors:

g++ -g -O0 junk.cpp -o junk -I /home/bradbell/prefix/cppad/include -I /home/bradbell/prefix/cppad/eigen/include -L/home/bradbell/prefix/cppad/lib64 -lipopt /usr/bin/ld: /usr/lib/gcc/x86_64-redhat-linux/9/../../../../lib64/crt1.o: in function _start': (.text+0x24): undefined reference tomain' /usr/bin/ld: /tmp/cc7Ypp7b.o: in function MPC::MPC()': /home/bradbell/trash/cppad_issue/junk.cpp:188: undefined reference tovtable for MPC'

[Tengyun-Mo]

I put your MPC.h at the front inline with at the top of the rest of your program in a file called junk.cpp and tried to compile it. I got the following errors:

g++ -g -O0 junk.cpp -o junk -I /home/bradbell/prefix/cppad/include -I /home/bradbell/prefix/cppad/eigen/include -L/home/bradbell/prefix/cppad/lib64 -lipopt /usr/bin/ld: /usr/lib/gcc/x86_64-redhat-linux/9/../../../../lib64/crt1.o: in function _start': (.text+0x24): undefined reference tomain' /usr/bin/ld: /tmp/cc7Ypp7b.o: in function MPC::MPC()': /home/bradbell/trash/cppad_issue/junk.cpp:188: undefined reference tovtable for MPC'

I made a demo to test mpc.cpp,and mpc.cpp is ok.There is the demo(maybe you should change some code in my demo,because I run the code in ROS):

#include <iostream>
#include <map>
#include <math.h>
#include <stdio.h>

#include "ros/ros.h"
#include "MPC.h"
#include <Eigen/Core>
#include <Eigen/QR>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv)
{
    //Initiate ROS
    ros::init(argc, argv, "MPC_Node");
    MPC mpc;

    VectorXd state(6);
    VectorXd coeffs(4);
    state << 0.394202, 0.0, 0.0, 3.94202, 0.743441, -0.0021541;
    coeffs << 0.744287, 0.00214534, 0.005139, 0.000007821;

    while(ros::ok()){

        vector<double> mpc_results = mpc.Solve(state, coeffs);
    }
    return 0;
}

[Tengyun-Mo]

Maybe I know where is the problem,It's an environmental configuration issue.After I reinstall ipopt package,everything is perfect.Thanks for your help @bradbell,I'm very appreciate it!

[hichamhendy]

Are there any considerations I need to take when I run that on ROS? I am facing the same troubles and reinstalling didn't change any thing!

[bradbell]

Is there someway I can reproduce your problem; e.g., clone a specific git repository and execute a specific stet of steps ?

[hichamhendy]

170