lqr_policy.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_LQR_POLICY_HPP_
#define ROBOTOC_LQR_POLICY_HPP_
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
 
 
namespace robotoc {
 
class LQRPolicy {
public:
  using MatrixXdRowMajor 
      = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
 
  LQRPolicy(const Robot& robot)
    : K(MatrixXdRowMajor::Zero(robot.dimu(), 2*robot.dimv())),
      k(Eigen::VectorXd::Zero(robot.dimu())),
      T(Eigen::VectorXd::Zero(robot.dimu())),
      W(Eigen::VectorXd::Zero(robot.dimu())),
      dimv_(robot.dimv()),
      dimu_(robot.dimu()) {
  }
 
  LQRPolicy() 
    : K(),
      k(),
      T(),
      W(),
      dimv_(0),
      dimu_(0) {
  }
 
  ~LQRPolicy() {
  }
 
  LQRPolicy(const LQRPolicy&) = default;
 
  LQRPolicy& operator=(const LQRPolicy&) = default;
 
  LQRPolicy(LQRPolicy&&) noexcept = default;
 
  LQRPolicy& operator=(LQRPolicy&&) noexcept = default;
 
  MatrixXdRowMajor K;
 
  Eigen::VectorXd k;
 
  Eigen::VectorXd T;
 
  Eigen::VectorXd W;
 
  const Eigen::Block<const MatrixXdRowMajor> Kq() const {
    return K.topLeftCorner(dimu_, dimv_);
  }
 
  const Eigen::Block<const MatrixXdRowMajor> Kv() const {
    return K.topRightCorner(dimu_, dimv_);
  }
 
  bool isApprox(const LQRPolicy& other) const {
    if (!K.isApprox(other.K)) return false;
    if (!k.isApprox(other.k)) return false;
    if (!T.isApprox(other.T)) return false;
    if (!W.isApprox(other.W)) return false;
    return true;
  }
 
private:
  int dimv_, dimu_;
 
};
 
} // namespace robotoc 
 
#endif // ROBOTOC_LQR_POLICY_HPP_