DDPSolver.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <array>
#include <functional>
#include <memory>
 
#include <nmpc_ddp/DDPProblem.h>
 
namespace nmpc_ddp
{
template<int StateDim, int InputDim>
class DDPSolver
{
public:
  using StateDimVector = typename DDPProblem<StateDim, InputDim>::StateDimVector;
 
  using InputDimVector = typename DDPProblem<StateDim, InputDim>::InputDimVector;
 
  using StateStateDimMatrix = typename DDPProblem<StateDim, InputDim>::StateStateDimMatrix;
 
  using InputInputDimMatrix = typename DDPProblem<StateDim, InputDim>::InputInputDimMatrix;
 
  using StateInputDimMatrix = typename DDPProblem<StateDim, InputDim>::StateInputDimMatrix;
 
  using InputStateDimMatrix = typename DDPProblem<StateDim, InputDim>::InputStateDimMatrix;
 
public:
  struct Configuration
  {
    Configuration()
    {
      // Initialize alpha_list
      int list_size = 11;
      alpha_list.resize(list_size);
      Eigen::VectorXd alpha_exponent_list = Eigen::VectorXd::LinSpaced(list_size, 0, -3);
      for(int i = 0; i < list_size; i++)
      {
        alpha_list[i] = std::pow(10, alpha_exponent_list[i]);
      }
    }
 
    int print_level = 1;
 
    // \todo Support use_state_eq_second_derivative
    bool use_state_eq_second_derivative = false;
 
    bool with_input_constraint = false;
 
    int max_iter = 500;
 
    int horizon_steps = 100;
 
    int reg_type = 1;
 
    double initial_lambda = 1e-4; // 1.0
 
    double initial_dlambda = 1.0;
 
    double lambda_factor = 1.6;
 
    double lambda_min = 1e-6;
 
    double lambda_max = 1e10;
 
    double k_rel_norm_thre = 1e-4;
 
    double lambda_thre = 1e-5;
 
    Eigen::VectorXd alpha_list;
 
    double cost_update_ratio_thre = 0;
 
    double cost_update_thre = 1e-7;
  };
 
  struct ControlData
  {
    std::vector<StateDimVector> x_list;
 
    std::vector<InputDimVector> u_list;
 
    Eigen::VectorXd cost_list;
  };
 
  struct Derivative
  {
    Derivative(int state_dim, int input_dim, int outer_dim)
    {
      Fx.resize(state_dim, state_dim);
      Fu.resize(state_dim, input_dim);
      Fxx.assign(outer_dim, StateStateDimMatrix(state_dim, state_dim));
      Fuu.assign(outer_dim, InputInputDimMatrix(input_dim, input_dim));
      Fxu.assign(outer_dim, StateInputDimMatrix(state_dim, input_dim));
      Lx.resize(state_dim);
      Lu.resize(input_dim);
      Lxx.resize(state_dim, state_dim);
      Luu.resize(input_dim, input_dim);
      Lxu.resize(state_dim, input_dim);
    }
 
    StateStateDimMatrix Fx;
 
    StateInputDimMatrix Fu;
 
    std::vector<StateStateDimMatrix> Fxx;
 
    std::vector<InputInputDimMatrix> Fuu;
 
    std::vector<StateInputDimMatrix> Fxu;
 
    StateDimVector Lx;
 
    InputDimVector Lu;
 
    StateStateDimMatrix Lxx;
 
    InputInputDimMatrix Luu;
 
    StateInputDimMatrix Lxu;
  };
 
  struct TraceData
  {
    int iter = 0;
 
    double cost = 0;
 
    double lambda = 0;
 
    double dlambda = 0;
 
    double alpha = 0;
 
    double k_rel_norm = 0;
 
    double cost_update_actual = 0;
 
    double cost_update_expected = 0;
 
    double cost_update_ratio = 0;
 
    double duration_derivative = 0;
 
    double duration_backward = 0;
 
    double duration_forward = 0;
  };
 
  struct ComputationDuration
  {
    double solve = 0;
 
    double setup = 0;
 
    double opt = 0;
 
    double derivative = 0;
 
    double backward = 0;
 
    double forward = 0;
 
    double Q = 0;
 
    double reg = 0;
 
    double gain = 0;
  };
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  DDPSolver(const std::shared_ptr<DDPProblem<StateDim, InputDim>> & problem);
 
  inline Configuration & config()
  {
    return config_;
  }
 
  inline const Configuration & config() const
  {
    return config_;
  }
 
  bool solve(double current_t, const StateDimVector & current_x, const std::vector<InputDimVector> & initial_u_list);
 
  inline void setInputLimitsFunc(const std::function<std::array<InputDimVector, 2>(double)> & input_limits_func)
  {
    input_limits_func_ = input_limits_func;
  }
 
  inline const ControlData & controlData() const
  {
    return control_data_;
  }
 
  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:
  int procOnce(int iter);
 
  bool backwardPass();
 
  void forwardPass(double alpha);
 
protected:
  Configuration config_;
 
  std::shared_ptr<DDPProblem<StateDim, InputDim>> problem_;
 
  std::vector<TraceData> trace_data_list_;
 
  std::function<std::array<InputDimVector, 2>(double)> input_limits_func_;
 
  ComputationDuration computation_duration_;
 
  double current_t_ = 0;
 
  double lambda_ = 0;
 
  double dlambda_ = 0;
 
  ControlData control_data_;
 
  ControlData candidate_control_data_;
 
  std::vector<InputDimVector> k_list_;
 
  std::vector<InputStateDimMatrix> K_list_;
 
  std::vector<Derivative> derivative_list_;
 
  StateDimVector last_Vx_;
 
  StateStateDimMatrix last_Vxx_;
 
  Eigen::Vector2d dV_;
};
} // namespace nmpc_ddp
 
#include <nmpc_ddp/DDPSolver.hpp>