snopt.hpp

Go to the documentation of this file.

/***********************************************************************************
Copyright (c) 2017, Diego Pardo. All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright notice,
      this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice,
      this list of conditions and the following disclaimer in the documentation
      and/or other materials provided with the distribution.
    * Neither the name of ETH ZURICH nor the names of its contributors may be used
      to endorse or promote products derived from this software without specific
      prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL ETH ZURICH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***************************************************************************************/

#ifndef INCLUDE_DTO_SNOPT_INTERFACE_HPP_
#define INCLUDE_DTO_SNOPT_INTERFACE_HPP_

#include <Eigen/Dense>
#include <solver_interfaces/base_solver_interface.hpp>
#include <snopt7/snoptProblem.hpp>

namespace DirectTrajectoryOptimization {

// forward declaration
template <typename DIMENSIONS>
class DTOMethodInterface;

template <class DIMENSIONS>
class DTOSnoptInterface : public DTOSolverInterface<DIMENSIONS> , public std::enable_shared_from_this<DTOSnoptInterface <DIMENSIONS> >
{

public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t;
    typedef typename DIMENSIONS::control_vector_array_t control_vector_array_t;

    DTOSnoptInterface(
        std::shared_ptr<DTOMethodInterface<DIMENSIONS> > method,
        bool snopt_verbose, bool feasible_point_only) :
            _method(method) {

    this->_verbose = snopt_verbose;

        if (this->_verbose) {
            minor_print_level_param = 0;
            major_print_level_param = 1;
        }

        _feasible_point_only = feasible_point_only;
    };

    virtual ~DTOSnoptInterface() {
        //interface_pointer = NULL;

        // delete variables if allocated
        if (use_computeJacobian) {
            delete []spt_iGfun;
            delete []spt_jGvar;
        }
    delete []spt_A;
    delete []spt_iAfun;
    delete []spt_jAvar;
        //delete []spt_variable_names;
        //delete []spt_Fnames;
    };


    static std::shared_ptr<DTOSnoptInterface <DIMENSIONS> > interface_pointer;  
    std::shared_ptr<DTOMethodInterface<DIMENSIONS> > _method;                   
    std::shared_ptr<snoptProblemA> nlp_solver;                                  
    snFunA spt_constraints_function = NULL;
    int status_output_flag = 0;
    bool use_computeJacobian = false;
    bool use_print_file = false;
    bool _feasible_point_only = false;


    static void NLP_function( int    *Status, int *n,    double x[],
              int    *needF,  int *neF,  double F[],
              int    *needG,  int *neG,  double G[],
              char   *cu,     int *lencu,
              int    iu[],    int *leniu,
              double ru[],    int *lenru );

    virtual void InitializeSolver(
            const std::string & problem_name,
            const std::string & output_file,
            const std::string & param_file = "",
            const bool & computeJacobian = false) override;
    void setupSNOPTvariables();
    void setupNLPsolver();
    void setupWarmStart();

    void estimateJacobianStructure();
    virtual void Solve(bool warminit=false) override;

    void setSparsityVariables();
    void setSnoptFLimits();

    void readParametersFromFile(const std::string & param_file);
    void SetupSolverParameters() override;

    void validateSNOPTStatus(const int & status,
            const Eigen::Map<Eigen::VectorXd> & x, const Eigen::Map<Eigen::VectorXd> & F);

    virtual void GetDTOSolution(
                state_vector_array_t &yTraj,
                control_vector_array_t &uTraj,
                Eigen::VectorXd &hTraj) override;
    virtual void GetDTOSolution(
            state_vector_array_t & y_trajectory,
            control_vector_array_t & u_trajectory,
            Eigen::VectorXd & h_trajectory,
            std::vector<int> & phaseId) override;

    virtual void GetDTOSolution(state_vector_array_t &yTraj,
      state_vector_array_t &ydotTraj, control_vector_array_t & uTraj,
      Eigen::VectorXd &hTraj, std::vector<int> &phaseId) override;

    virtual void GetFVector(Eigen::VectorXd &fVec) override {fVec = F_vector;}

    void GetSolverSolutionVariables(
            Eigen::VectorXi & _x_state,
            Eigen::VectorXd & _x_mul,
            Eigen::VectorXi & _f_state,
            Eigen::VectorXd & _f_mul) override;

    void InitializeDecisionVariablesFromTrajectory(
            const state_vector_array_t & y_sol,
            const control_vector_array_t & u_sol,
            const Eigen::VectorXd & h_sol) override;

    void WarmSolverInitialization(const Eigen::VectorXi & x_state,
            const Eigen::VectorXd & x_mul,
            const Eigen::VectorXi & f_state,
            const Eigen::VectorXd & f_mul) override;

    void SetVerifyLevel(const int & level) override {
        if(level > -2 && level < 4)
            verify_level_param = level;
        else {
            std::cout << "incorrect verification level!" << std::endl;
            std::exit(EXIT_FAILURE);
        }
    }

    void UsePrintFile(const bool & flag) override {
        use_print_file = flag;
    }

private:
    char* spt_name_of_problem = NULL;
    char* spt_name_of_output_file = NULL;
    char* spt_name_of_print_file=NULL;
    //char** spt_variable_names = NULL;
    //char* spt_Fnames = NULL;

    int spt_n_vars  = 0;
    int spt_lenF    = 0;
    int spt_obj_row = 0;
    int spt_obj_add = 0;
    int spt_neG     = 0;
    int jgcost_size = 0;
    int original_size_G = 0;

    int spt_neA = 0;
    int spt_lenA = 1;

    double * spt_A = NULL;

    int * spt_iGfun = NULL;
    int * spt_jGvar = NULL;

    int * spt_iAfun = NULL;
    int * spt_jAvar = NULL;

    double * spt_xlb = NULL;
    double * spt_xub = NULL;

    double * spt_Flb = NULL;
    double * spt_Fub = NULL;

    Eigen::VectorXd x_variables;
    Eigen::VectorXd x_variables_validation;
    Eigen::VectorXd x_multipliers;
    Eigen::VectorXi x_state;

    Eigen::VectorXd F_vector;
    Eigen::VectorXd F_vector_validation;
    Eigen::VectorXd F_multipliers;
    Eigen::VectorXi F_state;

    int snopt_warm_flag = 0;

    Eigen::VectorXd snopt_Flb_v;
    Eigen::VectorXd snopt_Fub_v;
    Eigen::VectorXi snopt_iGfun;
    Eigen::VectorXi snopt_jGvar;

    //  Derivative option
    //  = 0  some or all of the gradient elements need not be assigned values in G
    //  = 1  subroutine usrfun should compute all derivatives of F(x)
    int derivative_option_param = 0;

    int scale_option_param = 1;
    int verify_level_param = -1;
    int major_iteration_limit_param = 80000;
    int minor_iteration_limit_param = 80000;
    int iteration_limit_param = 40000000;
    double major_optimality_tolerance_param = 1e-1;
    double major_feasibility_tolerance_param = 1e-6;
    double minor_feasibility_tolerance_param = 1e-6;
  int print_file_param = 0;
  int minor_print_level_param = 0;
  int major_print_level_param = 0;
  int backup_basis_file_param = 0;
  double line_search_tolerance_param = 0.9;

    std::string print_solution_param = "No";
};

template<class DIMENSIONS>
std::shared_ptr<DTOSnoptInterface<DIMENSIONS> > DTOSnoptInterface<DIMENSIONS>::interface_pointer;

template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::InitializeSolver(const std::string & problem_name,
        const std::string & output_file, const std::string & param_file /*= "" */,
        const bool &computeJacobian) {

    use_computeJacobian = computeJacobian;

    // initialize Direct Transcription problem
    _method->Initialize();

    std::string print_file_name = output_file;
    std::string real_output_file = output_file + ".out";

    spt_name_of_problem = const_cast<char*>(problem_name.c_str());
    spt_name_of_output_file = const_cast<char*>(real_output_file.c_str());
    spt_name_of_print_file = const_cast<char*>(print_file_name.c_str());

    // Size of variables, limites, sparsity and names
    setupSNOPTvariables();

    if (param_file!="") {
        readParametersFromFile(param_file);
    }

    nlp_solver = std::shared_ptr<snoptProblemA>(new snoptProblemA(spt_name_of_problem));

    // Tolerance, iterations, print level, verify level, line search
    SetupSolverParameters();
}

template<class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::setupSNOPTvariables() {

    interface_pointer = std::enable_shared_from_this<DTOSnoptInterface<DIMENSIONS>>::shared_from_this();
    spt_constraints_function = &this->DTOSnoptInterface::NLP_function;

    // number of decision variables
    spt_n_vars = _method->myNLP.GetNumVars();

    // F includes the objective in SNOPT
    spt_lenF = _method->myNLP.GetNumF() + 1;
    spt_xlb = _method->myNLP.GetXlb_p();
    spt_xub = _method->myNLP.GetXub_p();

    x_variables.resize(spt_n_vars);
    x_variables_validation.resize(spt_n_vars);
    x_variables.setZero();
    x_multipliers.resize(spt_n_vars);
    x_state.resize(spt_n_vars);
    x_state.setZero();

    F_vector.resize(spt_lenF);
    F_vector_validation.resize(spt_lenF);
    F_multipliers.resize(spt_lenF);
    F_state.resize(spt_lenF);

    setSnoptFLimits();
    setSparsityVariables();
}

template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::setSnoptFLimits() {

    Eigen::VectorXd local_Flb = _method->myNLP.GetFlb();
    Eigen::VectorXd local_Fub = _method->myNLP.GetFub();

    snopt_Flb_v.resize(spt_lenF);
    snopt_Fub_v.resize(spt_lenF);

    snopt_Flb_v(0) = -infy;
    snopt_Fub_v(0) = infy;

    snopt_Flb_v.segment(1,spt_lenF-1) = local_Flb;
    snopt_Fub_v.segment(1,spt_lenF-1) = local_Fub;

    spt_Flb = snopt_Flb_v.data();
    spt_Fub = snopt_Fub_v.data();

    if (this->_verbose) {
        std::cout << "[VERBOSE] spt_lenF  : " << spt_lenF << std::endl;
        std::cout << "[VERBOSE] snopt_Flb : " << snopt_Flb_v.transpose() << std::endl;
        std::cout << "[VERBOSE] snopt_Fub : " << snopt_Fub_v.transpose() << std::endl;
        std::getchar();
    }
}

template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::setSparsityVariables() {

    Eigen::VectorXi local_iGfun = _method->myNLP.GetiGfun();
    Eigen::VectorXi local_jGvar = _method->myNLP.GetjGvar();
    Eigen::VectorXi local_jGcost =_method->myNLP.GetjGvarCost();

    jgcost_size = local_jGcost.size();
    original_size_G = _method->myNLP.GetDimG();

    /* linear parts are unknown */
        spt_lenA = spt_lenF*spt_n_vars;
        spt_iAfun = new int[spt_lenA];
        spt_jAvar = new int[spt_lenA];
        spt_A = new double[spt_lenA];

    if (use_computeJacobian) {

        spt_neG = spt_lenF*spt_n_vars;
        spt_iGfun = new int[spt_neG];
        spt_jGvar = new int[spt_neG];
    } else {

        spt_neG = original_size_G + jgcost_size;
        snopt_iGfun.resize(spt_neG);
        snopt_jGvar.resize(spt_neG);

        snopt_iGfun.segment(0,jgcost_size) = Eigen::VectorXi::Zero(jgcost_size);
        snopt_iGfun.segment(jgcost_size,original_size_G) = local_iGfun + Eigen::VectorXi::Ones(local_iGfun.size());
        snopt_jGvar.segment(0,jgcost_size) = local_jGcost;
        snopt_jGvar.segment(jgcost_size,original_size_G) = local_jGvar;

        spt_iGfun = snopt_iGfun.data();
        spt_jGvar = snopt_jGvar.data();
    }

    if (this->_verbose) {
        std::cout << "[VERBOSE] jgcost_size : " << jgcost_size << std::endl;
        std::cout << "[VERBOSE] spt_neG     : " << spt_neG << std::endl;
        std::getchar();
        std::cout << "[VERBOSE] snopt_iGfun : " << snopt_iGfun.transpose() << std::endl;
        std::getchar();
        std::cout << "[VERBOSE] snopt_jGvar : " << snopt_jGvar.transpose() << std::endl;
        std::getchar();
    }
}

template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::setupNLPsolver() {


    nlp_solver->setProblemSize( spt_n_vars , spt_lenF  );
    nlp_solver->setObjective  ( spt_obj_row , spt_obj_add );
    nlp_solver->setA              ( spt_lenA, spt_neA, spt_iAfun, spt_jAvar, spt_A);
    nlp_solver->setG          ( spt_neG, spt_neG, spt_iGfun, spt_jGvar );
    nlp_solver->setX          ( x_variables.data() , spt_xlb , spt_xub , x_multipliers.data() , x_state.data());
    nlp_solver->setF          ( F_vector.data(), spt_Flb , spt_Fub , F_multipliers.data(), F_state.data() );
    //test removing this
    //nlp_solver->setProbName     (spt_name_of_problem);
    nlp_solver->setUserFun    (spt_constraints_function);

    if(use_print_file) {
        nlp_solver->setPrintFile(spt_name_of_print_file);
    }
    if (use_computeJacobian) {
        this->estimateJacobianStructure();
    }
}

template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::SetupSolverParameters() {

    std::cout << std::endl << "setting parameters..." << std::endl;

    nlp_solver->setIntParameter("Scale option" , scale_option_param);

    //  Derivative option
    //  = 0  some or all of the gradient elements need not be assigned values in G
    //  = 1  subroutine usrfun should compute all derivatives of F(x)
    if (use_computeJacobian)
        derivative_option_param = 0;
    else
        derivative_option_param = 1;

    // tolerance for the nonlinear constraints
    nlp_solver->setRealParameter("Major feasibility tolerance", major_feasibility_tolerance_param);

    // tolerance for the variables bounds and linear constraints
    nlp_solver->setRealParameter("Minor feasibility tolerance", minor_feasibility_tolerance_param);

    nlp_solver->setIntParameter("Derivative option", derivative_option_param);
    nlp_solver->setIntParameter("Verify level" , verify_level_param);
    nlp_solver->setIntParameter("Major iterations limit", major_iteration_limit_param);
    nlp_solver->setIntParameter("Minor iterations limit", minor_iteration_limit_param);
    nlp_solver->setIntParameter("Iterations limit", iteration_limit_param);
    nlp_solver->setRealParameter("Major optimality tolerance", major_optimality_tolerance_param);
    nlp_solver->setIntParameter("Minor print level", minor_print_level_param);
    nlp_solver->setIntParameter("Major print level", major_print_level_param);

    // reading and saving basis files
    nlp_solver->setIntParameter("New basis file", this->new_basis_file_param);
    nlp_solver->setIntParameter("Old basis file", this->old_basis_file_param);
    nlp_solver->setIntParameter("Backup basis file", backup_basis_file_param);

    // only saving at the end
    nlp_solver->setIntParameter("Save frequency",100000000);

    if(this->_totally_quiet_) {
        nlp_solver->setParameter("Solution No");
        nlp_solver->setIntParameter("Summary file",0);
    }
    // Do not set these parameters as they crash with the filename
    // nlp_solver->setIntParameter("Solution file",0);
    // nlp_solver->setIntParameter("Print file",print_file_param);

    nlp_solver->setRealParameter("Linesearch tolerance", line_search_tolerance_param);
    //nlp_solver->setParameter("Hessian limited memory");
    //nlp_solver->setParameter("Hessian full memory");

    if (_feasible_point_only) {
        nlp_solver->setIntParameter("Feasible point only",1);
      nlp_solver->setParameter("Feasible point");
    }

}


template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::estimateJacobianStructure() {

    nlp_solver->computeJac(spt_neA,spt_neG);

    /* user may decide to stop here */
    std::cout << "Jacobian Structure has been estimated" << std::endl;
    std::cout << "please press [ENTER] if you want to continue with optimization" << std::endl;
    getchar();
}

template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::Solve(bool warminit) {
    std::cout << "Ready to solve... " << std::endl;

    // Only setup the NLP solver just before solving
    setupNLPsolver();

    if (warminit)
        snopt_warm_flag = 2;

    nlp_solver->solve(snopt_warm_flag);

    if(this->x_variables_validation.size() < 1){
        std::cout << "Internal error!" << std::endl;
        return;
    }
    // just to be sure SNOPT is not making stupid things!
    double epsilon_e = 1e-5;
    for(auto i = 0 ; i < this->spt_n_vars ; ++i) {
        if(std::fabs(this->x_variables(i) - this->x_variables_validation(i)) > epsilon_e) {
            std::cout << "SNOPT BUG! : X_variables are not the same!" << std::endl;
            std::cout << "x_variables("<< i <<") : " << x_variables(i) << std::endl;
            std::cout << "x_validation("<< i <<") : " << x_variables_validation(i) << std::endl;
        }
    }

    for(auto i = 0 ; i < this->spt_lenF; ++i) {
        if(std::fabs(this->F_vector(i) - this->F_vector_validation(i)) > epsilon_e) {
            std::cout << "SNOPT BUG! : Constraints are not consistent" << std::endl;
      std::cout << "F_vector("<< i <<") : " << F_vector(i) << std::endl;
      std::cout << "F_validation("<< i <<") : " << F_vector_validation(i) << std::endl;
      std::cout << "F_lb("<< i <<") : " << this->snopt_Flb_v(i) << std::endl;
      std::cout << "F_ub("<< i <<") : " << this->snopt_Fub_v(i) << std::endl;
        }
    }

}

//This should be in "METHOD CLASS"
//template <class DIMENSIONS>
//void DTOSnoptInterface<DIMENSIONS>::setXVariableNames() {
//
//  int names_array_length = spt_n_vars; // * 8;
//
//  spt_variable_names = new char*[spt_n_vars];
//
//
//  std::string base_index = "h_";
//  std::string base_stat = "s_";
//  std::string base_cont = "c_";
//
//  Eigen::VectorXi h_indexes;
//  Eigen::MatrixXi y_indexes;
//  Eigen::MatrixXi u_indexes;
//
//  _method->getTrajectoryIndexes(h_indexes,y_indexes,u_indexes);
//
//  int increment = 0;
//  int state = 0;
//  int control = 0;
//  int current_node = 0;
//  // The indexes
//
//  for (int i = 0; i < spt_n_vars; i++)    {
//
//      spt_variable_names[i] = new char[8];
//
//      std::string name;
//
//      if (i < y_indexes(0,0)) {
//          name = base_index + std::to_string(increment);
//          increment++;
//
//      } else if (i < u_indexes(0,0)) {
//          name = base_stat + std::to_string(state) + "_" + std::to_string(current_node);
//          state++;
//          if(state > y_indexes.rows()) {
//              state = 0;
//              current_node++;
//          }
//          if(current_node > y_indexes.cols())
//              current_node=0;
//
//      } else {
//          name = base_cont + std::to_string(control) + "_" + std::to_string(current_node);
//          control++;
//          if(control > u_indexes.rows()) {
//              control = 0;
//              current_node++;
//          }
//          if(current_node > u_indexes.cols())
//              current_node=0;
//      }
//
//      spt_variable_names[i] = const_cast<char*>(name.c_str());
//  }

//  spt_Fnames = new char[8];
//}

template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::readParametersFromFile(const std::string & param_file) {
    //ToDo : update using cereal?
}

template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::validateSNOPTStatus(const int & status,
        const Eigen::Map<Eigen::VectorXd> & X, const Eigen::Map<Eigen::VectorXd> & F) {

    /*Status = 2 , optimal solution
        Status = 3 , problem is infeasible
        Status = 4 , unbounded
        Status = 5 , iterations limit */

    std::string message;

    switch (status) {
        case 2:
            message = "OPTIMAL SOLUTION FOUND!!!!";
            break;
        case 3:
            message = "Problem is Infeasible :-(";
            break;
        case 4:
            message = "---> Unbounded <---";
            break;
        case 5:
            message = "-> Iteration Limit <-";
            break;
    }

    std::cout << message << std::endl;

    this->F_vector_validation = F;
    this->x_variables_validation = X;
}

template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::GetSolverSolutionVariables( Eigen::VectorXi & _x_state,
        Eigen::VectorXd & _x_mul, Eigen::VectorXi & _f_state, Eigen::VectorXd & _f_mul) {

    _x_state = this->x_state;
    _x_mul = this->x_multipliers;

    _f_state = this->F_state;
    _f_mul = this->F_multipliers;
}


template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::InitializeDecisionVariablesFromTrajectory( const state_vector_array_t & y_sol,
        const control_vector_array_t & u_sol, const Eigen::VectorXd &h_sol) {

    _method->SetDecisionVariablesFromStateControlIncrementsTrajectories(this->x_variables,y_sol,u_sol,h_sol);

}

template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::WarmSolverInitialization(const Eigen::VectorXi & p_x_state,
    const Eigen::VectorXd & p_x_mul, const Eigen::VectorXi & p_f_state,
    const Eigen::VectorXd & p_f_mul) {

    if(p_x_state.size() == spt_n_vars) {
        x_state = p_x_state;
        x_multipliers = p_x_mul;
        F_state = p_f_state;
        F_multipliers = p_f_mul;
    } else {
        std::cout << "Error during SNOPT solver initialization" << std::endl;
        std::exit(EXIT_FAILURE);
    }
}

template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::GetDTOSolution(state_vector_array_t & y_trajectory,
        control_vector_array_t & u_trajectory, Eigen::VectorXd & h_trajectory) {
    Eigen::Map<Eigen::VectorXd> x_vector(x_variables.data(),spt_n_vars);

    _method->getStateControlIncrementsTrajectoriesFromDecisionVariables(x_vector,y_trajectory,u_trajectory,h_trajectory);
}

template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::GetDTOSolution(state_vector_array_t & y_trajectory,
        control_vector_array_t & u_trajectory, Eigen::VectorXd & h_trajectory,
        std::vector<int> & phaseId) {

    this->GetDTOSolution(y_trajectory, u_trajectory, h_trajectory);

    phaseId = _method->node_to_phase_map;
}

template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::GetDTOSolution(state_vector_array_t &yTraj, state_vector_array_t &ydotTraj,
                    control_vector_array_t & uTraj, Eigen::VectorXd &hTraj, std::vector<int> &phaseId) {

  //Eigen::Map<Eigen::VectorXd> x_vector(x_variables.data(),spt_n_vars);

  _method->UpdateDecisionVariables(x_variables.data());
  _method->getCompleteSolution(yTraj,ydotTraj,uTraj,hTraj,phaseId);

}

template <class DIMENSIONS>
void DTOSnoptInterface<DIMENSIONS>::NLP_function( int    *Status, int *n,    double x[],
          int    *needF,  int *neF,  double F[],
          int    *needG,  int *neG,  double G[],
          char   *cu,     int *lencu,
          int    iu[],    int *leniu,
          double ru[],    int *lenru ) {

    DTOSnoptInterface<DIMENSIONS>::interface_pointer->_method->UpdateDecisionVariables(x);

    // compute G(x)
    if (*needG > 0 && !(DTOSnoptInterface<DIMENSIONS>::interface_pointer->use_computeJacobian)) {

        //This is here for debugging
        // int size_of_gcost = DTOSnoptInterface<DIMENSIONS>::interface_pointer->jgcost_size;
        // int size_of_G = DTOSnoptInterface<DIMENSIONS>::interface_pointer->original_size_G;
        //      int total_size_of_G = size_of_gcost + size_of_G;
        //      if(*neG > total_size_of_G || *neG < total_size_of_G) {
        //          std::cout << "BIG ERROR!" << std::endl;
        //          std::exit(EXIT_FAILURE);
        //      }

        DTOSnoptInterface<DIMENSIONS>::interface_pointer->_method->evalSparseJacobianCost(G);
        DTOSnoptInterface<DIMENSIONS>::interface_pointer->_method->evalSparseJacobianConstraint(G+DTOSnoptInterface<DIMENSIONS>::interface_pointer->jgcost_size);
    }

    // compute F(x)
    if (*needF > 0) {
        DTOSnoptInterface<DIMENSIONS>::interface_pointer->_method->evalObjective(&F[0]);
        DTOSnoptInterface<DIMENSIONS>::interface_pointer->_method->evalConstraintFct(&F[1], *neF - 1);
    }

    /*
        Status = 0  : there is nothing special about the current call to usrfun
        Status = 1  : snOptA is calling your subroutine for the FIST time
        Status >= 2 : snOptA is calling your subroutine for the LAST time
     */
    if (*Status > 1) {
        Eigen::Map<Eigen::VectorXd> F_b(F,*neF);
        Eigen::Map<Eigen::VectorXd> X_vector(x,*n);
        DTOSnoptInterface<DIMENSIONS>::interface_pointer->status_output_flag = *Status;
        DTOSnoptInterface<DIMENSIONS>::interface_pointer->validateSNOPTStatus(*Status,X_vector,F_b);
    }
}

} // namespace DirectTrajectoryOptimization

#endif /*INCLUDE_DTO_SNOPT_INTERFACE_HPP_*/