FmpcSolver.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <array>
#include <functional>
#include <memory>
 
#include <nmpc_fmpc/FmpcProblem.h>
 
namespace nmpc_fmpc
{
template<int StateDim, int InputDim, int IneqDim>
class FmpcSolver
{
public:
  using StateDimVector = typename FmpcProblem<StateDim, InputDim, IneqDim>::StateDimVector;
 
  using InputDimVector = typename FmpcProblem<StateDim, InputDim, IneqDim>::InputDimVector;
 
  using IneqDimVector = typename FmpcProblem<StateDim, InputDim, IneqDim>::IneqDimVector;
 
  using StateStateDimMatrix = typename FmpcProblem<StateDim, InputDim, IneqDim>::StateStateDimMatrix;
 
  using InputInputDimMatrix = typename FmpcProblem<StateDim, InputDim, IneqDim>::InputInputDimMatrix;
 
  using StateInputDimMatrix = typename FmpcProblem<StateDim, InputDim, IneqDim>::StateInputDimMatrix;
 
  using InputStateDimMatrix = typename FmpcProblem<StateDim, InputDim, IneqDim>::InputStateDimMatrix;
 
  using IneqStateDimMatrix = typename FmpcProblem<StateDim, InputDim, IneqDim>::IneqStateDimMatrix;
 
  using IneqInputDimMatrix = typename FmpcProblem<StateDim, InputDim, IneqDim>::IneqInputDimMatrix;
 
  using IneqIneqDimMatrix = Eigen::Matrix<double, IneqDim, IneqDim>;
 
public:
  struct Configuration
  {
    int print_level = 1;
 
    int horizon_steps = 100;
 
    int max_iter = 10;
 
    double kkt_error_thre = 1e-4;
 
    bool check_nan = true;
 
    bool init_complementary_variable = false;
 
    bool update_barrier_eps = true;
 
    bool break_if_llt_fails = false;
 
    bool enable_line_search = false;
 
    bool merit_const_scale_from_lagrange_multipliers = false;
  };
 
  enum class Status
  {
    Uninitialized = 0,
 
    Succeeded = 1,
 
    ErrorInForward = 2,
 
    ErrorInBackward = 3,
 
    ErrorInUpdate = 4,
 
    MaxIterationReached = 5,
 
    IterationContinued = 6
  };
 
  struct Variable
  {
    Variable(int _horizon_steps = 0);
 
    void reset(double _x, double _u, double _lambda, double _s, double _nu);
 
    bool containsNaN() const;
 
    int horizon_steps;
 
    std::vector<StateDimVector> x_list;
 
    std::vector<InputDimVector> u_list;
 
    std::vector<StateDimVector> lambda_list;
 
    std::vector<IneqDimVector> s_list;
 
    std::vector<IneqDimVector> nu_list;
 
    int print_level = 1;
  };
 
  struct Coefficient
  {
    Coefficient(int state_dim, int input_dim, int ineq_dim);
 
    Coefficient(int state_dim);
 
    bool containsNaN() const;
 
    StateStateDimMatrix A;
 
    StateInputDimMatrix B;
 
    IneqStateDimMatrix C;
 
    IneqInputDimMatrix D;
 
    StateDimVector Lx;
 
    InputDimVector Lu;
 
    StateStateDimMatrix Lxx;
 
    InputInputDimMatrix Luu;
 
    StateInputDimMatrix Lxu;
 
    StateDimVector x_bar;
    IneqDimVector g_bar;
    StateDimVector Lx_bar;
    InputDimVector Lu_bar;
 
    InputDimVector k;
 
    InputStateDimMatrix K;
 
    StateDimVector s;
 
    StateStateDimMatrix P;
 
    int print_level = 1;
  };
 
  struct TraceData
  {
    int iter = 0;
 
    double kkt_error = 0;
 
    double duration_coeff = 0;
 
    double duration_backward = 0;
 
    double duration_forward = 0;
 
    double duration_update = 0;
  };
 
  struct ComputationDuration
  {
    double solve = 0;
 
    double setup = 0;
 
    double opt = 0;
 
    double coeff = 0;
 
    double backward = 0;
 
    double forward = 0;
 
    double update = 0;
 
    double gain_pre = 0;
 
    double gain_solve = 0;
 
    double gain_post = 0;
 
    double fraction = 0;
  };
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  FmpcSolver(const std::shared_ptr<FmpcProblem<StateDim, InputDim, IneqDim>> & problem) : problem_(problem) {}
 
  inline Configuration & config()
  {
    return config_;
  }
 
  inline const Configuration & config() const
  {
    return config_;
  }
 
  Status solve(double current_t, const StateDimVector & current_x, const Variable & initial_variable);
 
  inline const Variable & variable() const
  {
    return variable_;
  }
 
  inline const std::vector<Coefficient> & coeffList() const
  {
    return coeff_list_;
  }
 
  inline const std::vector<TraceData> & traceDataList() const
  {
    return trace_data_list_;
  }
 
  inline const ComputationDuration & computationDuration() const
  {
    return computation_duration_;
  }
 
  void dumpTraceDataList(const std::string & file_path) const;
 
protected:
  void checkVariable() const;
 
  Status procOnce(int iter);
 
  double calcKktError(double barrier_eps) const;
 
  bool backwardPass();
 
  bool forwardPass();
 
  bool updateVariables();
 
  void setupMeritFunc();
 
  double calcMeritFunc(const Variable & variable) const;
 
protected:
  Configuration config_;
 
  std::shared_ptr<FmpcProblem<StateDim, InputDim, IneqDim>> problem_;
 
  Variable variable_;
 
  Variable delta_variable_;
 
  std::vector<Coefficient> coeff_list_;
 
  std::vector<TraceData> trace_data_list_;
 
  ComputationDuration computation_duration_;
 
  double current_t_ = 0;
 
  StateDimVector current_x_ = StateDimVector::Zero();
 
  double barrier_eps_ = 1e-4;
 
  double merit_const_scale_ = 0.0;
 
  double merit_func_ = 0.0;
 
  double merit_deriv_ = 0.0;
};
} // namespace nmpc_fmpc
 
#include <nmpc_fmpc/FmpcSolver.hpp>