split_kkt_matrix.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_SPLIT_KKT_MATRIX_HPP_
#define ROBOTOC_SPLIT_KKT_MATRIX_HPP_
 
#include <iostream>
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/robot/contact_status.hpp"
 
 
namespace robotoc {
 
class SplitKKTMatrix {
public:
  SplitKKTMatrix(const Robot& robot);
 
  SplitKKTMatrix();
 
  ~SplitKKTMatrix() = default;
 
  SplitKKTMatrix(const SplitKKTMatrix&) = default;
 
  SplitKKTMatrix& operator=(const SplitKKTMatrix&) = default;
 
  SplitKKTMatrix(SplitKKTMatrix&&) noexcept = default;
 
  SplitKKTMatrix& operator=(SplitKKTMatrix&&) noexcept = default;
 
  void setContactDimension(const int dimf);
 
  void setSwitchingConstraintDimension(const int dims);
 
  Eigen::MatrixXd Fxx;
 
  Eigen::Block<Eigen::MatrixXd> Fqq();
 
  const Eigen::Block<const Eigen::MatrixXd> Fqq() const;
 
  Eigen::Block<Eigen::MatrixXd> Fqv();
 
  const Eigen::Block<const Eigen::MatrixXd> Fqv() const;
 
  Eigen::Block<Eigen::MatrixXd> Fvq();
 
  const Eigen::Block<const Eigen::MatrixXd> Fvq() const;
 
  Eigen::Block<Eigen::MatrixXd> Fvv();
 
  const Eigen::Block<const Eigen::MatrixXd> Fvv() const;
 
  Eigen::MatrixXd Fvu;
 
  Eigen::VectorXd fx;
 
  Eigen::VectorBlock<Eigen::VectorXd> fq();
 
  const Eigen::VectorBlock<const Eigen::VectorXd> fq() const;
 
  Eigen::VectorBlock<Eigen::VectorXd> fv();
 
  const Eigen::VectorBlock<const Eigen::VectorXd> fv() const;
 
  Eigen::Block<Eigen::MatrixXd> Phix();
 
  const Eigen::Block<const Eigen::MatrixXd> Phix() const;
 
  Eigen::Block<Eigen::MatrixXd> Phiq();
 
  const Eigen::Block<const Eigen::MatrixXd> Phiq() const;
 
  Eigen::Block<Eigen::MatrixXd> Phiv();
 
  const Eigen::Block<const Eigen::MatrixXd> Phiv() const;
 
  Eigen::Block<Eigen::MatrixXd> Phia();
 
  const Eigen::Block<const Eigen::MatrixXd> Phia() const;
 
  Eigen::Block<Eigen::MatrixXd> Phiu();
 
  const Eigen::Block<const Eigen::MatrixXd> Phiu() const;
 
  Eigen::VectorBlock<Eigen::VectorXd> Phit();
 
  const Eigen::VectorBlock<const Eigen::VectorXd> Phit() const;
 
 
  Eigen::MatrixXd Qxx;
 
  Eigen::Block<Eigen::MatrixXd> Qqq();
 
  const Eigen::Block<const Eigen::MatrixXd> Qqq() const;
 
  Eigen::Block<Eigen::MatrixXd> Qqv();
 
  const Eigen::Block<const Eigen::MatrixXd> Qqv() const;
 
  Eigen::Block<Eigen::MatrixXd> Qvq();
 
  const Eigen::Block<const Eigen::MatrixXd> Qvq() const;
 
  Eigen::Block<Eigen::MatrixXd> Qvv();
 
  const Eigen::Block<const Eigen::MatrixXd> Qvv() const;
 
  Eigen::MatrixXd Qaa;
 
  Eigen::MatrixXd Qdvdv;
 
  Eigen::MatrixXd Qxu;
 
  Eigen::Block<Eigen::MatrixXd> Qqu();
 
  const Eigen::Block<const Eigen::MatrixXd> Qqu() const;
 
  Eigen::Block<Eigen::MatrixXd> Qvu();
 
  const Eigen::Block<const Eigen::MatrixXd> Qvu() const;
 
  Eigen::MatrixXd Quu;
 
  Eigen::Block<Eigen::MatrixXd> Qff();
 
  const Eigen::Block<const Eigen::MatrixXd> Qff() const;
 
  Eigen::Block<Eigen::MatrixXd> Qqf();
 
  const Eigen::Block<const Eigen::MatrixXd> Qqf() const;
 
  double Qtt;
 
  double Qtt_prev;
 
  Eigen::VectorXd hx;
 
  Eigen::VectorBlock<Eigen::VectorXd> hq();
 
  const Eigen::VectorBlock<const Eigen::VectorXd> hq() const;
 
  Eigen::VectorBlock<Eigen::VectorXd> hv();
 
  const Eigen::VectorBlock<const Eigen::VectorXd> hv() const;
 
  Eigen::VectorXd hu;
 
  Eigen::VectorXd ha;
 
  Eigen::VectorBlock<Eigen::VectorXd> hf();
 
  const Eigen::VectorBlock<const Eigen::VectorXd> hf() const;
 
  void setZero();
 
  int dimf() const;
 
  int dims() const;
 
 
  bool isDimensionConsistent() const;
 
  bool isApprox(const SplitKKTMatrix& other) const;
 
  bool hasNaN() const;
 
  void setRandom();
 
  void setRandom(const ContactStatus& contact_status);
 
  static SplitKKTMatrix Random(const Robot& robot);
 
  static SplitKKTMatrix Random(const Robot& robot, 
                               const ContactStatus& contact_status);
 
  void disp(std::ostream& os) const;
 
  friend std::ostream& operator<<(std::ostream& os, 
                                  const SplitKKTMatrix& kkt_matrix);
 
private:
  Eigen::MatrixXd Phix_full_, Phia_full_, Phiu_full_;
  Eigen::VectorXd Phit_full_;
  Eigen::MatrixXd Qff_full_, Qqf_full_;
  Eigen::VectorXd hf_full_;
  bool has_floating_base_;
  int dimv_, dimx_, dimu_, dimf_, dims_;
 
};
 
} // namespace robotoc 
 
#include "robotoc/core/split_kkt_matrix.hxx"
 
#endif // ROBOTOC_SPLIT_KKT_MATRIX_HPP_