LinearMpcZ.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <functional>
 
#include <qp_solver_collection/QpSolverCollection.h>
 
#include <CCC/VariantSequentialExtension.h>
 
namespace CCC
{
class LinearMpcZ
{
public:
  static constexpr int state_dim_ = 2;
 
  using _StateSpaceModel = StateSpaceModel<state_dim_, Eigen::Dynamic, Eigen::Dynamic>;
 
  using StateDimVector = _StateSpaceModel::StateDimVector;
 
public:
  using InitialParam = Eigen::Vector2d;
 
  struct WeightParam
  {
    double pos;
 
    double force;
 
    WeightParam(double _pos = 1.0, double _force = 1e-7) : pos(_pos), force(_force) {}
  };
 
  class ModelContactPhase : public _StateSpaceModel
  {
  public:
    ModelContactPhase(double mass);
  };
 
  class ModelNoncontactPhase : public _StateSpaceModel
  {
  public:
    ModelNoncontactPhase(double mass);
  };
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  LinearMpcZ(double mass,
             double horizon_dt,
             int horizon_steps,
             const WeightParam & weight_param = WeightParam(),
             QpSolverCollection::QpSolverType qp_solver_type = QpSolverCollection::QpSolverType::Any);
 
  double planOnce(const std::function<bool(double)> & contact_func,
                  const std::function<double(double)> & ref_pos_func,
                  const InitialParam & initial_param,
                  double current_time);
 
protected:
  double procOnce(const std::vector<std::shared_ptr<_StateSpaceModel>> & model_list,
                  const StateDimVector & current_x,
                  const Eigen::VectorXd & ref_pos_seq);
 
public:
  double mass_ = 0;
 
  double horizon_dt_ = 0;
 
  int horizon_steps_ = 0;
 
  WeightParam weight_param_;
 
  std::shared_ptr<_StateSpaceModel> model_contact_;
 
  std::shared_ptr<_StateSpaceModel> model_noncontact_;
 
  std::shared_ptr<QpSolverCollection::QpSolver> qp_solver_;
 
  QpSolverCollection::QpCoeff qp_coeff_;
 
  std::pair<double, double> force_range_;
};
} // namespace CCC