riccati_factorizer.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_RICCATI_FACTORIZER_HPP_ 
#define ROBOTOC_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/core/split_direction.hpp"
#include "robotoc/riccati/split_riccati_factorization.hpp"
#include "robotoc/riccati/lqr_policy.hpp"
#include "robotoc/riccati/sto_policy.hpp"
#include "robotoc/riccati/backward_riccati_recursion_factorizer.hpp"
#include "robotoc/riccati/split_constrained_riccati_factorization.hpp"
 
#include <limits>
#include <cmath>
 
 
namespace robotoc {
 
class RiccatiFactorizer {
public:
  RiccatiFactorizer(const Robot& robot, const double max_dts0=0.1);
 
  RiccatiFactorizer();
 
  ~RiccatiFactorizer();
 
  RiccatiFactorizer(const RiccatiFactorizer&) = default;
 
  RiccatiFactorizer& operator=(const RiccatiFactorizer&) = default;
 
  RiccatiFactorizer(RiccatiFactorizer&&) noexcept = default;
 
  RiccatiFactorizer& operator=(RiccatiFactorizer&&) noexcept = default;
 
  void setRegularization(const double max_dts0);
 
  void backwardRiccatiRecursion(const SplitRiccatiFactorization& riccati_next, 
                                SplitKKTMatrix& kkt_matrix, 
                                SplitKKTResidual& kkt_residual,  
                                SplitRiccatiFactorization& riccati,
                                LQRPolicy& lqr_policy);
 
  void backwardRiccatiRecursion(const SplitRiccatiFactorization& riccati_next, 
                                SplitKKTMatrix& kkt_matrix, 
                                SplitKKTResidual& kkt_residual,  
                                SplitRiccatiFactorization& riccati,
                                LQRPolicy& lqr_policy, const bool sto,
                                const bool has_next_sto_phase);
 
  void backwardRiccatiRecursionPhaseTransition(
      const SplitRiccatiFactorization& riccati, 
      SplitRiccatiFactorization& riccati_m, STOPolicy& sto_policy, 
      const bool has_next_sto_phase) const;
 
  void backwardRiccatiRecursion(const SplitRiccatiFactorization& riccati_next, 
                                SplitKKTMatrix& kkt_matrix, 
                                SplitKKTResidual& kkt_residual, 
                                SplitRiccatiFactorization& riccati);
 
  void backwardRiccatiRecursion(const SplitRiccatiFactorization& riccati_next, 
                                SplitKKTMatrix& kkt_matrix, 
                                SplitKKTResidual& kkt_residual, 
                                SplitRiccatiFactorization& riccati,
                                const bool sto);
 
private:
  bool has_floating_base_;
  int dimv_, dimu_;
  double max_dts0_, eps_;
  Eigen::LLT<Eigen::MatrixXd> llt_, llt_s_;
  LQRPolicy lqr_policy_;
  BackwardRiccatiRecursionFactorizer backward_recursion_;
  SplitConstrainedRiccatiFactorization c_riccati_;
 
};
 
 
void forwardRiccatiRecursion(const SplitKKTMatrix& kkt_matrix, 
                             const SplitKKTResidual& kkt_residual,
                             const LQRPolicy& lqr_policy, SplitDirection& d, 
                             SplitDirection& d_next, const bool sto,
                             const bool has_next_sto_phase);
 
void forwardRiccatiRecursion(const SplitKKTMatrix& kkt_matrix, 
                            const SplitKKTResidual& kkt_residual,
                            const SplitDirection& d, 
                            SplitDirection& d_next);
 
void computeSwitchingTimeDirection(const STOPolicy& sto_policy,
                                   SplitDirection& d,
                                   const bool has_prev_sto_phase);
 
void computeCostateDirection(const SplitRiccatiFactorization& riccati, 
                             SplitDirection& d, const bool sto, 
                             const bool has_next_sto_phase);
 
void computeCostateDirection(const SplitRiccatiFactorization& riccati, 
                             SplitDirection& d, const bool sto);
 
void computeLagrangeMultiplierDirection(
    const SplitRiccatiFactorization& riccati, SplitDirection& d,
    const bool sto, const bool has_next_sto_phase);
 
} // namespace robotoc
 
#endif // ROBOTOC_RICCATI_FACTORIZER_HPP_