IntrinsicallyStableMpc.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <functional>
 
#include <qp_solver_collection/QpSolverCollection.h>
 
namespace CCC
{
class IntrinsicallyStableMpc1d
{
  friend class IntrinsicallyStableMpc;
 
public:
  struct RefData
  {
    double zmp = 0;
 
    std::array<double, 2> zmp_limits;
  };
 
  struct InitialParam
  {
    double capture_point = 0;
 
    double planned_zmp = 0;
  };
 
  struct WeightParam
  {
    double zmp;
 
    double zmp_vel;
 
    WeightParam(double _zmp = 1.0, double _zmp_vel = 1e-3) : zmp(_zmp), zmp_vel(_zmp_vel) {}
  };
 
public:
  IntrinsicallyStableMpc1d(double com_height,
                           double horizon_duration,
                           double horizon_dt,
                           QpSolverCollection::QpSolverType qp_solver_type = QpSolverCollection::QpSolverType::Any,
                           const WeightParam & weight_param = WeightParam());
 
  double planOnce(const std::function<RefData(double)> & ref_data_func,
                  const InitialParam & initial_param,
                  double current_time,
                  double control_dt = -1);
 
protected:
  double procOnce(const std::vector<RefData> & ref_data_seq,
                  const InitialParam & initial_param,
                  double current_time,
                  double control_dt);
 
protected:
  WeightParam weight_param_;
 
  double horizon_dt_ = 0;
 
  int horizon_steps_ = -1;
 
  std::shared_ptr<QpSolverCollection::QpSolver> qp_solver_;
 
  QpSolverCollection::QpCoeff qp_coeff_;
 
  double omega_ = 0;
 
  double lambda_ = 0;
 
  Eigen::MatrixXd P_;
};
 
class IntrinsicallyStableMpc
{
public:
  struct RefData
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector2d zmp = Eigen::Vector2d::Zero();
 
    std::array<Eigen::Vector2d, 2> zmp_limits;
  };
 
  struct InitialParam
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector2d capture_point = Eigen::Vector2d::Zero();
 
    Eigen::Vector2d planned_zmp = Eigen::Vector2d::Zero();
  };
 
public:
  IntrinsicallyStableMpc(
      double com_height,
      double horizon_duration,
      double horizon_dt,
      QpSolverCollection::QpSolverType qp_solver_type = QpSolverCollection::QpSolverType::Any,
      const IntrinsicallyStableMpc1d::WeightParam & weight_param = IntrinsicallyStableMpc1d::WeightParam())
  : mpc_1d_(
      std::make_shared<IntrinsicallyStableMpc1d>(com_height, horizon_duration, horizon_dt, qp_solver_type, weight_param))
  {
    ref_data_seq_x_.resize(mpc_1d_->horizon_steps_);
    ref_data_seq_y_.resize(mpc_1d_->horizon_steps_);
  }
 
  Eigen::Vector2d planOnce(const std::function<RefData(double)> & ref_data_func,
                           const InitialParam & initial_param,
                           double current_time,
                           double control_dt = -1);
 
protected:
  std::shared_ptr<IntrinsicallyStableMpc1d> mpc_1d_;
 
  std::vector<IntrinsicallyStableMpc1d::RefData> ref_data_seq_x_;
 
  std::vector<IntrinsicallyStableMpc1d::RefData> ref_data_seq_y_;
};
} // namespace CCC