unconstr_backward_correction.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_UNCONSTR_BACKWARD_CORRECTION_HPP_
#define ROBOTOC_UNCONSTR_BACKWARD_CORRECTION_HPP_
 
#include <vector>
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/utils/aligned_vector.hpp"
#include "robotoc/core/split_kkt_matrix.hpp"
#include "robotoc/core/split_kkt_residual.hpp"
#include "robotoc/core/split_solution.hpp"
#include "robotoc/core/split_direction.hpp"
#include "robotoc/core/solution.hpp"
#include "robotoc/core/direction.hpp"
#include "robotoc/core/kkt_matrix.hpp"
#include "robotoc/core/kkt_residual.hpp"
#include "robotoc/ocp/ocp.hpp"
#include "robotoc/ocp/grid_info.hpp"
#include "robotoc/parnmpc/unconstr_split_backward_correction.hpp"
#include "robotoc/unconstr/unconstr_ocp_data.hpp"
#include "robotoc/unconstr/parnmpc_intermediate_stage.hpp"
#include "robotoc/unconstr/parnmpc_terminal_stage.hpp"
 
 
namespace robotoc {
 
class UnconstrBackwardCorrection {
public:
  UnconstrBackwardCorrection(const OCP& ocp, const int nthreads);
 
  UnconstrBackwardCorrection();
 
  ~UnconstrBackwardCorrection() = default;
 
  UnconstrBackwardCorrection(const UnconstrBackwardCorrection&) = default;
 
  UnconstrBackwardCorrection& operator=(const UnconstrBackwardCorrection&) = default;
 
  UnconstrBackwardCorrection(UnconstrBackwardCorrection&&) noexcept = default;
 
  UnconstrBackwardCorrection& operator=(UnconstrBackwardCorrection&&) noexcept = default;
 
  void setNumThreads(const int nthreads);
 
  void initAuxMat(aligned_vector<Robot>& robots, 
                  const std::vector<GridInfo>& time_discretization,
                  const Solution& s, KKTMatrix& kkt_matrix, 
                  KKTResidual& kkt_residual);
 
  void initConstraints(aligned_vector<Robot>& robots, 
                       const std::vector<GridInfo>& time_discretization, 
                       const Solution& s);
 
  void evalOCP(aligned_vector<Robot>& robots, 
               const std::vector<GridInfo>& time_discretization, 
               const Eigen::VectorXd& q, const Eigen::VectorXd& v, 
               const Solution& s, KKTResidual& kkt_residual);
 
  void evalKKT(aligned_vector<Robot>& robots, 
               const std::vector<GridInfo>& time_discretization, 
               const Eigen::VectorXd& q, const Eigen::VectorXd& v, 
               const Solution& s, KKTMatrix& kkt_matrix, 
               KKTResidual& kkt_residual);
 
  void coarseUpdate(aligned_vector<Robot>& robots, 
                    const std::vector<GridInfo>& time_discretization,
                    const Eigen::VectorXd& q, const Eigen::VectorXd& v, 
                    const Solution& s, KKTMatrix& kkt_matrix, 
                    KKTResidual& kkt_residual);
 
  void backwardCorrection(const std::vector<GridInfo>& time_discretization,
                          const Solution& s, const KKTMatrix& kkt_matrix, 
                          const KKTResidual& kkt_residual, Direction& d);
 
  double primalStepSize() const;
 
  double dualStepSize() const;
 
  const PerformanceIndex& getEval() const {
    return performance_index_;
  }
 
  void integrateSolution(const aligned_vector<Robot>& robots,
                         const std::vector<GridInfo>& time_discretization, 
                         const double primal_step_size, 
                         const double dual_step_size, Direction& d, Solution& s);
 
  void integratePrimalSolution(const aligned_vector<Robot>& robots,
                               const std::vector<GridInfo>& time_discretization, 
                               const double primal_step_size, 
                               const Direction& d, Solution& s);
 
private:
  int nthreads_;
  OCP ocp_;
  ParNMPCIntermediateStage intermediate_stage_;
  ParNMPCTerminalStage terminal_stage_;
  aligned_vector<UnconstrOCPData> data_;
  PerformanceIndex performance_index_;
  aligned_vector<UnconstrSplitBackwardCorrection> corrector_;
  Solution s_new_;
  std::vector<Eigen::MatrixXd> aux_mat_;
  Eigen::VectorXd primal_step_sizes_, dual_step_sizes_;
 
};
 
} // namespace robotoc
 
#endif // ROBOTOC_UNCONSTR_BACKWARD_CORRECTION_HPP_