unconstr_riccati_factorizer.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_UNCONSTR_RICCATI_FACTORIZER_HPP_ 
#define ROBOTOC_UNCONSTR_RICCATI_FACTORIZER_HPP_
 
#include "Eigen/Core"
#include "Eigen/LU"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/core/split_kkt_matrix.hpp"
#include "robotoc/core/split_kkt_residual.hpp"
#include "robotoc/core/split_direction.hpp"
#include "robotoc/riccati/split_riccati_factorization.hpp"
#include "robotoc/riccati/lqr_policy.hpp"
#include "robotoc/riccati/unconstr_backward_riccati_recursion_factorizer.hpp"
 
#include <limits>
#include <cmath>
 
 
namespace robotoc {
 
class UnconstrRiccatiFactorizer {
public:
  UnconstrRiccatiFactorizer(const Robot& robot);
 
  UnconstrRiccatiFactorizer();
 
  ~UnconstrRiccatiFactorizer();
 
  UnconstrRiccatiFactorizer(const UnconstrRiccatiFactorizer&) = default;
 
  UnconstrRiccatiFactorizer& operator=(
      const UnconstrRiccatiFactorizer&) = default;
 
  UnconstrRiccatiFactorizer(UnconstrRiccatiFactorizer&&) noexcept = default;
 
  UnconstrRiccatiFactorizer& operator=(
      UnconstrRiccatiFactorizer&&) noexcept = default;
 
  void backwardRiccatiRecursion(const SplitRiccatiFactorization& riccati_next, 
                                const double dt, SplitKKTMatrix& kkt_matrix, 
                                SplitKKTResidual& kkt_residual,  
                                SplitRiccatiFactorization& riccati,
                                LQRPolicy& lqr_policy);
 
  void forwardRiccatiRecursion(const SplitKKTResidual& kkt_residual,
                               const double dt, const LQRPolicy& lqr_policy,
                               SplitDirection& d,  SplitDirection& d_next) const;
 
  static void computeCostateDirection(const SplitRiccatiFactorization& riccati, 
                                      SplitDirection& d);
 
private:
  int dimv_;
  Eigen::LLT<Eigen::MatrixXd> llt_;
  UnconstrBackwardRiccatiRecursionFactorizer backward_recursion_;
 
};
 
} // namespace robotoc
 
#endif // ROBOTOC_UNCONSTR_RICCATI_FACTORIZER_HPP_