ipopt.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 DT_IPOPT_INTERFACE_HPP_
#define DT_IPOPT_INTERFACE_HPP_

#include <solver_interfaces/base_solver_interface.hpp>
#include <IpIpoptApplication.hpp>
#include <IpTNLP.hpp>

using namespace Ipopt;

namespace DirectTrajectoryOptimization {

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

template<typename DIMENSIONS>
class DTOIpoptInterface:  public TNLP , public DTOSolverInterface<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;

    DTOIpoptInterface(std::shared_ptr<DTOMethodInterface<DIMENSIONS> > method,
            bool ipopt_verbose) :
            _method(method) {
    this->_verbose = ipopt_verbose;
        _nlpSolver = new Ipopt::IpoptApplication();
    }

    ~DTOIpoptInterface() {

        /* Workaround*/
        /* the shared pointer must destroy everything, including the Ipopt SmartPtr */
            Ipopt::Referencer* t=NULL;
            this->ReleaseRef(t);
            delete t;
    }

    virtual void InitializeSolver(const std::string & problemName,
            const std::string & outputFile,
            const std::string & paramFile /*= "" */,
            const bool &computeJacobian) override;

    virtual void Solve(bool warminit = false) override;

    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 GetFVector(Eigen::VectorXd &fVec) override;

//  void WarmInitialization(
//          const state_vector_array_t & y_sol,
//          const control_vector_array_t & u_sol,
//          const Eigen::VectorXd & h_sol,
//          const Eigen::VectorXi & x_state,
//          const Eigen::VectorXd & x_mul,
//          const Eigen::VectorXi & f_state,
//          const Eigen::VectorXd & f_mul) override;

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

    void SetupSolverParameters() override;

private:

    // private helper functions
    void SetupIpoptVariables();
    void SetupNLP();


    void SetSparsityVariables();

    void readParametersFromFile(const std::string & p_file);

    // IPOPT functions
    virtual bool get_nlp_info(Index& n, Index& m, Index& nnz_jac_g,
            Index& nnz_h_lag, IndexStyleEnum& index_style) override;

    virtual bool get_bounds_info(Index n, Number* x_l, Number* x_u, Index m,
            Number *g_l, Number* g_u) override;

    virtual bool get_starting_point(Index n, bool init_x, Number* x,
            bool init_z, Number* z_L, Number* z_U, Index m, bool init_lambda,
            Number* lambda) override;

    virtual bool eval_f(Index n, const Number* x, bool new_x, Number& obj_value)
            override;

    virtual bool eval_grad_f(Index n, const Number* x, bool new_x,
            Number* grad_f) override;

    virtual bool eval_g(Index n, const Number* x, bool new_x, Index m,
            Number* g) override;

    virtual bool eval_jac_g(Index n, const Number* x, bool new_x, Index m,
            Index nele_jac, Index* iRow, Index* jCol, Number* values) override;

    virtual void finalize_solution(SolverReturn status, Index n,
            const Number* x, const Number* z_L, const Number* z_U, Index m,
            const Number* g, const Number* lambda, Number obj_value,
            const IpoptData* ip_data, IpoptCalculatedQuantities* ip_cq)
                    override;

    // private member variables
    bool use_computeJacobian = false;

    Ipopt::SmartPtr<IpoptApplication> _nlpSolver;
    std::shared_ptr<DTOMethodInterface<DIMENSIONS> > _method;

    int _lenX           = 0;            // size of decision variables vector
    int _lenConstraint  = 0;            // size of constraints vector

    int _nnzCostJac     = 0;            // number of non-zero elements of cost jacobian
    int _nnzConstraintJac = 0;          // number of non-zero elements of constraints jacobian

    Eigen::VectorXd _valX;
    double _valCost = 0.0;              // current value of cost
    Eigen::VectorXd _valConstraint;     // current value of constraints

    Eigen::VectorXd _valCostJac;        // current value of cost jacobian
    Eigen::VectorXd _valConstraintJac;  // current value of constraint jacobian
    Eigen::VectorXd _lamdaConstraints;

    Eigen::VectorXi _jCostJac;          // row indices of cost jacobian
    Eigen::VectorXi _iConstraintJac;    // row indices of constraint jacobian
    Eigen::VectorXi _jConstraintJac;    // col indices of constraint jacobian

    // output parameters
    std::string ippt_name_of_problem    = "";
    std::string ippt_name_of_output_file= "";
    double ippt_print_level             = 5;
    double ippt_file_print_level        = 5;
    std::string ippt_print_user_options         = "no";
    std::string ippt_print_options_documentation= "no";
    std::string ippt_print_timing_statistics    = "yes";

    // convergence parameters
    double ippt_tolerance           = 1e-4;
    double ippt_max_iter            = 10000;
    double ippt_max_cpu_time        = 1e6;
    std::string ippt_mu_strategy    = "adaptive";           // monotone, adaptive
    double ippt_dual_inf_tol        = 1;
    double ippt_constr_viol_tol     = 1e-6;
    double ippt_compl_inf_tol       = 1e-6;
    double ippt_acceptable_tol      = 1e-6;
    double ippt_acceptable_iter     = 30;

    // NLP Scaling parameters
    std::string ippt_nlp_scaling_method = "gradient-based";  //none, gradient-based, user-scaling, equilibration-based (only with MC19)
    double ippt_obj_scaling_factor      = 1;
    double ippt_nlp_scaling_max_gradient= 100;          // if max grad above this value, scaling will be performed
    double ippt_nlp_scaling_obj_target_gradient = 0;

    // linear solver parameters
    std::string ippt_linear_solver          = "ma57";       // ma27/57/77/86/97, pardiso, wsmp, mumps, custom
    std::string ippt_fast_step_computation  = "no";         // yes

    // derivative checker parameters
    std::string ippt_derivative_test    = "none";  // none, first-order
    double ippt_derivative_test_tol     = 1e-4;
    double point_perturbation_radius    = 0.01;
    //double    ippt_derivative_test_perturbation = std::sqrt(Eigen::NumTraits<double>::epsilon());
//  double  ippt_derivative_test_perturbation = std::sqrt(Eigen::NumTraits<double>::epsilon());

    // Hessian approximation parameters
    std::string ippt_hessian_approximation = "limited-memory"; // (exact) <- if implemented in code
    double ippt_limited_memory_max_history = 6;
    double ippt_limited_memory_max_skipping= 2;

    bool ippt_rethrowNonIpoptException = false;
};

template<typename DIMENSIONS>
void DTOIpoptInterface<DIMENSIONS>::InitializeSolver(
        const std::string & problemName,
        const std::string & outputFile,
        const std::string & paramFile /*= "" */,
        const bool &computeJacobian) {
    use_computeJacobian = computeJacobian;
    ippt_name_of_problem = problemName;
    ippt_name_of_output_file = outputFile;

    _method->Initialize();  // ensure that the method variables are ready

    SetupIpoptVariables();
    SetupNLP();

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

    SetupSolverParameters();

    return;
}

template<typename DIMENSIONS>
void DTOIpoptInterface<DIMENSIONS>::SetupIpoptVariables() {

    _lenX = _method->myNLP.GetNumVars();
    _lenConstraint = _method->myNLP.GetNumF();
    _valConstraint.resize(_lenConstraint);
}

template<typename DIMENSIONS>
void DTOIpoptInterface<DIMENSIONS>::SetupNLP() {

    SetSparsityVariables();
    return;
}

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

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

}


template<typename DIMENSIONS>
void DTOIpoptInterface<DIMENSIONS>::SetupSolverParameters() {

    // OUTPUT
    _nlpSolver->Options()->SetStringValue("output_file", ippt_name_of_output_file);
    _nlpSolver->Options()->SetIntegerValue("print_level", ippt_print_level);
    _nlpSolver->Options()->SetIntegerValue("file_print_level", ippt_file_print_level);
    _nlpSolver->Options()->SetStringValue("print_user_options", ippt_print_user_options);
    _nlpSolver->Options()->SetStringValue("print_options_documentation", ippt_print_options_documentation);
    _nlpSolver->Options()->SetStringValue("print_timing_statistics", ippt_print_timing_statistics);

    // CONVERGENCE
    _nlpSolver->Options()->SetNumericValue("tol",ippt_tolerance);
    _nlpSolver->Options()->SetIntegerValue("max_iter",ippt_max_iter);
    _nlpSolver->Options()->SetNumericValue("max_cpu_time",ippt_max_cpu_time);
    _nlpSolver->Options()->SetStringValue("mu_strategy",ippt_mu_strategy);
    _nlpSolver->Options()->SetNumericValue("dual_inf_tol",ippt_dual_inf_tol);
    _nlpSolver->Options()->SetNumericValue("constr_viol_tol", ippt_constr_viol_tol);
    _nlpSolver->Options()->SetNumericValue("compl_inf_tol", ippt_compl_inf_tol);
    _nlpSolver->Options()->SetNumericValue("acceptable_tol", ippt_acceptable_tol);
    _nlpSolver->Options()->SetIntegerValue("acceptable_iter", ippt_acceptable_iter);

    // NLP Scaling
    _nlpSolver->Options()->SetStringValue("nlp_scaling_method", ippt_nlp_scaling_method);
    _nlpSolver->Options()->SetNumericValue("obj_scaling_factor", ippt_obj_scaling_factor);
    _nlpSolver->Options()->SetNumericValue("nlp_scaling_max_gradient", ippt_nlp_scaling_max_gradient);
    _nlpSolver->Options()->SetNumericValue("nlp_scaling_obj_target_gradient", ippt_nlp_scaling_obj_target_gradient);

    // LINEAR SOLVER
    _nlpSolver->Options()->SetStringValue("linear_solver", ippt_linear_solver);
    _nlpSolver->Options()->SetStringValue("fast_step_computation", ippt_fast_step_computation);

    // DERIVATIVE checker
    _nlpSolver->Options()->SetStringValue("derivative_test", ippt_derivative_test);
    _nlpSolver->Options()->SetNumericValue("derivative_test_tol", ippt_derivative_test_tol);
    _nlpSolver->Options()->SetNumericValue("point_perturbation_radius", point_perturbation_radius);
    //_nlpSolver->Options()->SetNumericValue("derivative_test_perturbation",ippt_derivative_test_perturbation);

    // HESSIAN approximation
    _nlpSolver->Options()->SetStringValue("hessian_approximation",ippt_hessian_approximation);
    _nlpSolver->Options()->SetIntegerValue("limited_memory_max_history",ippt_limited_memory_max_history);
    _nlpSolver->Options()->SetIntegerValue("limited_memory_max_skipping",ippt_limited_memory_max_skipping);

    _nlpSolver->RethrowNonIpoptException(ippt_rethrowNonIpoptException);

    if (use_computeJacobian) {
        _nlpSolver->Options()->SetStringValue("jacobian_approximation",
                "finite-difference-values");
    }
}

template<typename DIMENSIONS>
void DTOIpoptInterface<DIMENSIONS>::readParametersFromFile(const std::string & paramFile) {

    //ToDo : Disabled after new base_method_interface
    //  _app->Options->SetStringValue("option_file_name", paramFile);
}

template<typename DIMENSIONS>
void DTOIpoptInterface<DIMENSIONS>::SetSparsityVariables() {

    // ipopt does not exploit sparsity in the cost function!
    _nnzCostJac = _lenX; // TODO inconsistent with the name nnz
    //but we need to map the indexes from the base method
     _jCostJac = _method->myNLP.GetjGvarCost();

    if (use_computeJacobian) {

        _nnzConstraintJac = _lenConstraint * _lenX;
        _iConstraintJac.resize(_nnzConstraintJac);
        _jConstraintJac.resize(_nnzConstraintJac);

        for (int i = 0; i < _nnzConstraintJac; i++) {
            _iConstraintJac[i] = i % _lenConstraint;
            _jConstraintJac[i] = floor(i/_lenConstraint);
        }
    } else {
        _nnzConstraintJac   = _method->myNLP.GetDimG();
        _iConstraintJac     = _method->myNLP.GetiGfun();
        _jConstraintJac     = _method->myNLP.GetjGvar();
    }

    _valX.resize(_lenX);
    _valCostJac.resize(_nnzCostJac);
    _valConstraintJac.resize(_nnzConstraintJac);

    // some assertions to make sure sizes are as we expect
    assert(_iConstraintJac.size() == _nnzConstraintJac);
    assert(_jConstraintJac.size() == _nnzConstraintJac);
//  assert(_jCostJac.size() == _nnzCostJac); Not true for IPOPT where nonspare version is used

    if (this->_verbose) {
        std::cout << "[VERBOSE] _iConstraintJac : " << _iConstraintJac.transpose() << std::endl;
        std::cout << "[VERBOSE] _jConstraintJac : " << _jConstraintJac.transpose() << std::endl;
        std::getchar();
    }

    return;
}

template<typename DIMENSIONS>
void DTOIpoptInterface<DIMENSIONS>::Solve(bool warminit) {

    ApplicationReturnStatus status;

    // Initialize the problem
    status = _nlpSolver->Initialize();

    if (status == Solve_Succeeded) {
        std::cout << std::endl << std::endl
                << "*** Initialized successfully -- starting NLP." << std::endl;
    } else {
        std::cout << std::endl << std::endl
                << "*** Error during initialization!" << std::endl;
        exit(EXIT_FAILURE);
    }

    // Solve this!
    status = _nlpSolver->OptimizeTNLP(this);

    if (status == Solve_Succeeded) {
        std::cout << std::endl << std::endl << "*** The problem solved!"
                << std::endl;
    } else {
        std::cout << std::endl << std::endl << "*** The problem FAILED!"
                << std::endl;
    }
}

template<typename DIMENSIONS>
void DTOIpoptInterface<DIMENSIONS>::GetDTOSolution(state_vector_array_t &yTraj,
        control_vector_array_t &uTraj, Eigen::VectorXd &hTraj) {

    Eigen::Map<Eigen::VectorXd> x_local(_valX.data(), _lenX);
    _method->getStateControlIncrementsTrajectoriesFromDecisionVariables(x_local, yTraj, uTraj, hTraj);

    return;
}

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

    Eigen::Map<Eigen::VectorXd> x_local(_valX.data(), _lenX);

    _method->getStateControlIncrementsTrajectoriesFromDecisionVariables(x_local,y_trajectory,u_trajectory,h_trajectory);

    phaseId = _method->node_to_phase_map;
}

template<typename DIMENSIONS>
void DTOIpoptInterface<DIMENSIONS>::GetFVector(Eigen::VectorXd &fVec) {

    fVec.resize(1 + _valConstraint.size());

    fVec(0) = _valCost;
    fVec.segment(1, _valConstraint.size()) = _valConstraint;

    return;
}

//template <class DIMENSIONS>
//void DTOIpoptInterface<DIMENSIONS>::WarmInitialization(   const state_vector_array_t & y_sol,
//  const control_vector_array_t & u_sol, const Eigen::VectorXd & h_sol,
//  const Eigen::VectorXi & p_x_state, const Eigen::VectorXd & p_x_mul,
//  const Eigen::VectorXi & p_f_state, const Eigen::VectorXd & p_f_mul) {
//
//  // for now we just support initialization of the constraint multipliers
//  this->_lamdaConstraints = p_f_mul;
//
//  _method->SetDecisionVariablesFromStateControlIncrementsTrajectories(this->_valX,y_sol,u_sol,h_sol);
//
//}

// IPOPT Functions
template<typename DIMENSIONS>
bool DTOIpoptInterface<DIMENSIONS>::get_nlp_info(Index& n, Index& m,
        Index& nnz_jac_g, Index& nnz_h_lag, IndexStyleEnum& index_style) {
    n = _lenX;
    m = _lenConstraint;

    nnz_jac_g = _nnzConstraintJac;
    nnz_h_lag = 0;

    index_style = TNLP::C_STYLE;

    return true;
}

template<typename DIMENSIONS>
bool DTOIpoptInterface<DIMENSIONS>::get_bounds_info(Index n, Number* x_l,
        Number* x_u, Index m, Number* g_l, Number* g_u) {

    Number *pointer;

    pointer = _method->myNLP.GetXlb_p();
    memcpy(x_l, pointer, sizeof(Number) * n);

    pointer = _method->myNLP.GetXub_p();
    memcpy(x_u, pointer, sizeof(Number) * n);

    pointer = _method->myNLP.GetFlb_p();
    memcpy(g_l, pointer, sizeof(Number) * m);

    pointer = _method->myNLP.GetFub_p();
    memcpy(g_u, pointer, sizeof(Number) * m);

    return true;
}

template<typename DIMENSIONS>
bool DTOIpoptInterface<DIMENSIONS>::get_starting_point(Index n, bool init_x,
        Number* x, bool init_z, Number* z_L, Number* z_U, Index m,
        bool init_lambda, Number* lambda) {
    std::cout << "getting starting points" << std::endl;

    // if these asserts fail, some options have been set that are not handled
    assert(init_z == false);
    if (init_z == true ){
        std::runtime_error("init_z should be false");
    }

    Eigen::Map<Eigen::VectorXd> local_x(x, n);
    Eigen::Map<Eigen::VectorXd> local_lamndaF(lambda, m);
    if (init_x){
        std::cout << "x init is true" << std::endl;
        local_x = this->_valX;
        std::cout << "x initialized" << std::endl;
    }
    if (init_lambda){
        std::runtime_error("init_lambda requested"); // TODO: Are these ever set?
        local_lamndaF = this->_lamdaConstraints;
    }

    this->_method->UpdateDecisionVariables(x);
//  std::cout << "Print X" << std::endl;
//  for (size_t i=0; i<n; i++){
//      std::cout << x[i] << std::endl;
//  }

    return true;
}

template<typename DIMENSIONS>
bool DTOIpoptInterface<DIMENSIONS>::eval_f(Index n, const Number* x, bool new_x,
        Number& obj_value) {
//  std::cout << "eval_f()" << std::endl;

//  std::cout << "Received decision variables" << std::endl;
//  std::cout << "Print X" << std::endl;
//  for (size_t i=0; i<n; i++){
//      std::cout << x[i] << std::endl;
//  }
//
//  std::cout << "y[0]" << std::endl;
//  std::cout << _method->_y_trajectory[0].transpose() << std::endl;
//  std::cout << "u[0]" << std::endl;
//  std::cout << _method->_u_trajectory[0].transpose() << std::endl;
//
//  std::cout << "y[1]" << std::endl;
//  std::cout << _method->_y_trajectory[1].transpose() << std::endl;
//  std::cout << "u[1]" << std::endl;
//  std::cout << _method->_u_trajectory[1].transpose() << std::endl;

    if (new_x) this->_method->UpdateDecisionVariables(x);

    this->_method->evalObjective(&obj_value);

    return true;
}

template<typename DIMENSIONS>
bool DTOIpoptInterface<DIMENSIONS>::eval_grad_f(Index n, const Number* x,
        bool new_x, Number* grad_f) {
//  std::cout << "eval_grad_f()" << std::endl;
    if (new_x) this->_method->UpdateDecisionVariables(x);

    if (use_computeJacobian)
        return true;

    Eigen::VectorXd local_sparse_cost_gradient(_jCostJac.size());
    this->_method->evalSparseJacobianCost(local_sparse_cost_gradient.data());

    // Set all to zero before filling in the sparsity -> TODO: only set the zero elements
    for (int i = 0 ; i < n; i++)
        grad_f[i] = 0;

    for (int i = 0 ; i < _jCostJac.size() ; i++)
        grad_f[_jCostJac[i]] = local_sparse_cost_gradient(i);

    return true;
}

template<typename DIMENSIONS>
bool DTOIpoptInterface<DIMENSIONS>::eval_g(Index n, const Number* x, bool new_x,
        Index m, Number* g) {
//  std::cout << "eval_g()" << std::endl;
    if (new_x) this->_method->UpdateDecisionVariables(x);

    this->_method->evalConstraintFct(g,m);

    return true;
}

template<typename DIMENSIONS>
bool DTOIpoptInterface<DIMENSIONS>::eval_jac_g(Index n, const Number* x,
        bool new_x, Index m, Index nele_jac, Index* iRow, Index* jCol,
        Number* values) {
//  std::cout << "eval_jac_g()" << std::endl;

    assert(nele_jac == _jConstraintJac.size());
    // Return sparsity structure
    if (values == NULL) {
        std::cout << "Return sparsity" << std::endl;
        // iPopt Bug: the second line produces an error!!
        //      std::memcpy(iRow,_iConstraintJac.data(),  sizeof(double) * nele_jac);
        //      std::memcpy(jCol,_jConstraintJac.data(),  sizeof(double) * nele_jac);

        for (int i = 0; i < nele_jac; i++) {
            iRow[i] = _iConstraintJac(i);
            jCol[i] = _jConstraintJac(i);
        }


    } else if (!use_computeJacobian){
        if (new_x) this->_method->UpdateDecisionVariables(x);
//      std::cout << "Computing Sparse Jacobian" << std::endl;
        this->_method->evalSparseJacobianConstraint(values);
//      Eigen::Map<Eigen::VectorXd> val_vec(values,this->_method->myNLP.lenG);
//      for (int i = 0; i < nele_jac; i++) {
//          std::cout << "(" << _iConstraintJac(i) << "," <<  _jConstraintJac(i) << ") " << val_vec[i] << std::endl;
//          std::getchar();
//      }

    }

    return true;
}

template<typename DIMENSIONS>
void DTOIpoptInterface<DIMENSIONS>::finalize_solution(SolverReturn status,
        Index n, const Number* x, const Number* z_L, const Number* z_U, Index m,
        const Number* g, const Number* lambda, Number obj_value,
        const IpoptData* ip_data, IpoptCalculatedQuantities* ip_cq) {
    this->_method->UpdateDecisionVariables(x);

    assert(n == _valX.size());
    assert(m == _valConstraint.size());

    memcpy(_valX.data(), x, n * sizeof(Number));
    _valCost = obj_value;
    memcpy(_valConstraint.data(), g, m * sizeof(Number));

    return;
}
} // namespace DirectTrajectoryOptimization
#endif /*DT_IPOPT_INTERFACE_HPP_*/