LinearMpcZmp.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <functional>
 
#include <qp_solver_collection/QpSolverCollection.h>
 
#include <CCC/CommonModels.h>
#include <CCC/InvariantSequentialExtension.h>
 
namespace CCC
{
class LinearMpcZmp1d
{
  friend class LinearMpcZmp;
 
public:
  struct RefData
  {
    std::array<double, 2> zmp_limits;
  };
 
  using InitialParam = Eigen::Vector3d;
 
public:
  LinearMpcZmp1d(double com_height,
                 double horizon_duration,
                 double horizon_dt,
                 QpSolverCollection::QpSolverType qp_solver_type = QpSolverCollection::QpSolverType::Any);
 
  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:
  double horizon_dt_ = 0;
 
  int horizon_steps_ = -1;
 
  std::shared_ptr<ComZmpModelJerkInput> model_;
 
  std::shared_ptr<InvariantSequentialExtension<3, 1, 1>> seq_ext_;
 
  std::shared_ptr<QpSolverCollection::QpSolver> qp_solver_;
 
  QpSolverCollection::QpCoeff qp_coeff_;
};
 
class LinearMpcZmp
{
public:
  struct RefData
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    std::array<Eigen::Vector2d, 2> zmp_limits;
  };
 
  struct InitialParam
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector2d pos = Eigen::Vector2d::Zero();
 
    Eigen::Vector2d vel = Eigen::Vector2d::Zero();
 
    Eigen::Vector2d acc = Eigen::Vector2d::Zero();
  };
 
public:
  LinearMpcZmp(double com_height,
               double horizon_duration,
               double horizon_dt,
               QpSolverCollection::QpSolverType qp_solver_type = QpSolverCollection::QpSolverType::Any)
  : mpc_1d_(std::make_shared<LinearMpcZmp1d>(com_height, horizon_duration, horizon_dt, qp_solver_type))
  {
    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<LinearMpcZmp1d> mpc_1d_;
 
  std::vector<LinearMpcZmp1d::RefData> ref_data_seq_x_;
 
  std::vector<LinearMpcZmp1d::RefData> ref_data_seq_y_;
};
} // namespace CCC