backward_riccati_recursion_factorizer.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_BACKWARD_RICCATI_RECURSION_FACTORIZER_HPP_ 
#define ROBOTOC_BACKWARD_RICCATI_RECURSION_FACTORIZER_HPP_
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/core/split_kkt_matrix.hpp"
#include "robotoc/core/split_kkt_residual.hpp"
#include "robotoc/riccati/split_riccati_factorization.hpp"
#include "robotoc/riccati/lqr_policy.hpp"
 
 
namespace robotoc {
 
class BackwardRiccatiRecursionFactorizer {
public:
  using MatrixXdRowMajor 
      = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
 
  BackwardRiccatiRecursionFactorizer(const Robot& robot);
 
  BackwardRiccatiRecursionFactorizer();
 
  ~BackwardRiccatiRecursionFactorizer();
 
  BackwardRiccatiRecursionFactorizer(
      const BackwardRiccatiRecursionFactorizer&) = default;
 
  BackwardRiccatiRecursionFactorizer& operator=(
      const BackwardRiccatiRecursionFactorizer&) = default;
 
  BackwardRiccatiRecursionFactorizer(
      BackwardRiccatiRecursionFactorizer&&) noexcept = default;
 
  BackwardRiccatiRecursionFactorizer& operator=(
      BackwardRiccatiRecursionFactorizer&&) noexcept = default;
 
  void factorizeKKTMatrix(const SplitRiccatiFactorization& riccati_next, 
                          SplitKKTMatrix& kkt_matrix,  
                          SplitKKTResidual& kkt_residual);
 
  void factorizeHamiltonian(const SplitRiccatiFactorization& riccati_next,
                            const SplitKKTMatrix& kkt_matrix, 
                            SplitRiccatiFactorization& riccati,
                            const bool has_next_sto_phase) const;
 
  void factorizeKKTMatrix(const SplitRiccatiFactorization& riccati_next, 
                          SplitKKTMatrix& kkt_matrix);
 
  void factorizeRiccatiFactorization(
      const SplitRiccatiFactorization& riccati_next, 
      SplitKKTMatrix& kkt_matrix, const SplitKKTResidual& kkt_residual, 
      const LQRPolicy& lqr_policy, SplitRiccatiFactorization& riccati);
 
  void factorizeSTOFactorization(
      const SplitRiccatiFactorization& riccati_next, 
      const SplitKKTMatrix& kkt_matrix, const SplitKKTResidual& kkt_residual, 
      const LQRPolicy& lqr_policy, SplitRiccatiFactorization& riccati,
      const bool has_next_sto_phase);
 
  void factorizeRiccatiFactorization(
      const SplitRiccatiFactorization& riccati_next, 
      const SplitKKTMatrix& kkt_matrix, 
      const SplitKKTResidual& kkt_residual, 
      SplitRiccatiFactorization& riccati);
 
  void factorizeSTOFactorization(
      const SplitRiccatiFactorization& riccati_next, 
      const SplitKKTMatrix& kkt_matrix, 
      const SplitKKTResidual& kkt_residual, 
      SplitRiccatiFactorization& riccati);
 
private:
  int dimv_, dimu_;
  MatrixXdRowMajor AtP_, BtP_;
  Eigen::MatrixXd GK_;
  Eigen::VectorXd Pf_;
 
};
 
} // namespace robotoc
 
#endif // ROBOTOC_BACKWARD_RICCATI_RECURSION_FACTORIZER_HPP_