unconstr_split_backward_correction.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_UNCONSTR_SPLIT_BACKWARD_CORRECTION_HPP_
#define ROBOTOC_UNCONSTR_SPLIT_BACKWARD_CORRECTION_HPP_
 
#include <vector>
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/core/split_solution.hpp"
#include "robotoc/core/split_direction.hpp"
#include "robotoc/core/split_kkt_matrix.hpp"
#include "robotoc/core/split_kkt_residual.hpp"
#include "robotoc/parnmpc/unconstr_kkt_matrix_inverter.hpp"
 
 
namespace robotoc {
 
class UnconstrSplitBackwardCorrection {
public:
  UnconstrSplitBackwardCorrection(const Robot& robot);
 
  UnconstrSplitBackwardCorrection();
 
  ~UnconstrSplitBackwardCorrection();
 
  UnconstrSplitBackwardCorrection(
      const UnconstrSplitBackwardCorrection&) = default;
 
  UnconstrSplitBackwardCorrection& operator=(
      const UnconstrSplitBackwardCorrection&) = default;
 
  UnconstrSplitBackwardCorrection(
      UnconstrSplitBackwardCorrection&&) noexcept = default;
 
  UnconstrSplitBackwardCorrection& operator=(
      UnconstrSplitBackwardCorrection&&) noexcept = default;
 
  void coarseUpdate(const Eigen::MatrixXd& aux_mat_next,
                    const double dt, const SplitKKTMatrix& kkt_matrix, 
                    const SplitKKTResidual& kkt_residual,
                    const SplitSolution& s, SplitSolution& s_new);
 
  void coarseUpdate(const Eigen::Block<Eigen::MatrixXd>& aux_mat_next,
                    const double dt, const SplitKKTMatrix& kkt_matrix, 
                    const SplitKKTResidual& kkt_residual,
                    const SplitSolution& s, SplitSolution& s_new);
 
  void coarseUpdate(const double dt, const SplitKKTMatrix& kkt_matrix, 
                    const SplitKKTResidual& kkt_residual,
                    const SplitSolution& s, SplitSolution& s_new);
 
  const Eigen::Block<const Eigen::MatrixXd> auxMat() const;
 
  void backwardCorrectionSerial(const SplitSolution& s_next, 
                                const SplitSolution& s_new_next,
                                SplitSolution& s_new);
 
  void backwardCorrectionParallel(SplitSolution& s_new);
 
  void forwardCorrectionSerial(const SplitSolution& s_prev, 
                               const SplitSolution& s_new_prev,
                               SplitSolution& s_new);
 
  void forwardCorrectionParallel(SplitSolution& s_new);
 
  static void computeDirection(const SplitSolution& s, 
                               const SplitSolution& s_new, 
                               SplitDirection& d);
 
private:
  int dimv_, dimx_, dimkkt_;
  UnconstrKKTMatrixInverter kkt_mat_inverter_;
  Eigen::MatrixXd H_, kkt_mat_inv_;
  Eigen::VectorXd kkt_res_, d_, x_res_, dx_;
 
 
  template <typename MatrixType>
  void coarseUpdate_impl(const Eigen::MatrixBase<MatrixType>& aux_mat_next, 
                         const double dt, const SplitKKTMatrix& kkt_matrix, 
                         const SplitKKTResidual& kkt_residual, 
                         const SplitSolution& s, SplitSolution& s_new) {
    assert(aux_mat_next.rows() == dimx_);
    assert(aux_mat_next.cols() == dimx_);
    H_.topLeftCorner(dimv_, dimv_)     = kkt_matrix.Qaa;
    H_.bottomLeftCorner(dimx_, dimv_)  = kkt_matrix.Qxu; // This is actually Qxa
    H_.bottomRightCorner(dimx_, dimx_) = kkt_matrix.Qxx.transpose();
    H_.bottomRightCorner(dimx_, dimx_).noalias() += aux_mat_next;
    kkt_mat_inverter_.invert(dt, H_, kkt_mat_inv_);
    kkt_res_.head(dimx_)           = kkt_residual.Fx;
    kkt_res_.segment(dimx_, dimv_) = kkt_residual.la;
    kkt_res_.tail(dimx_)           = kkt_residual.lx;
    d_.noalias() = kkt_mat_inv_ * kkt_res_;
    s_new.lmd = s.lmd - d_.head(dimv_);
    s_new.gmm = s.gmm - d_.segment(dimv_, dimv_);
    s_new.a   = s.a   - d_.segment(2*dimv_, dimv_); 
    s_new.q   = s.q   - d_.segment(3*dimv_, dimv_);
    s_new.v   = s.v   - d_.tail(dimv_);
  }
 
};
 
} // namespace robotoc
 
#endif // ROBOTOC_UNCONSTR_SPLIT_BACKWARD_CORRECTION_HPP_