LinearMpcXY.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <functional>
 
#include <SpaceVecAlg/SpaceVecAlg>
 
#include <qp_solver_collection/QpSolverCollection.h>
 
#include <CCC/VariantSequentialExtension.h>
 
namespace ForceColl
{
class Contact;
}
 
namespace CCC
{
class LinearMpcXY
{
public:
  static constexpr int state_dim_ = 6;
 
  using _StateSpaceModel = StateSpaceModel<state_dim_, Eigen::Dynamic, Eigen::Dynamic>;
 
  using StateDimVector = _StateSpaceModel::StateDimVector;
 
public:
  struct MotionParam
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    double com_z;
 
    double total_force_z;
 
    std::vector<std::shared_ptr<ForceColl::Contact>> contact_list;
  };
 
  struct InitialParam
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector2d pos = Eigen::Vector2d::Zero();
 
    Eigen::Vector2d vel = Eigen::Vector2d::Zero();
 
    Eigen::Vector2d angular_momentum = Eigen::Vector2d::Zero();
 
    StateDimVector toState(double mass) const;
  };
 
  struct RefData
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector2d pos = Eigen::Vector2d::Zero();
 
    Eigen::Vector2d vel = Eigen::Vector2d::Zero();
 
    Eigen::Vector2d angular_momentum = Eigen::Vector2d::Zero();
 
    static constexpr int outputDim()
    {
      return 6;
    }
 
    StateDimVector toOutput(double mass) const;
  };
 
  struct WeightParam
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector2d linear_momentum_integral;
 
    Eigen::Vector2d linear_momentum;
 
    Eigen::Vector2d angular_momentum;
 
    double force;
 
    WeightParam(const Eigen::Vector2d & _linear_momentum_integral = Eigen::Vector2d::Constant(1.0),
                const Eigen::Vector2d & _linear_momentum = Eigen::Vector2d::Constant(0.0),
                const Eigen::Vector2d & _angular_momentum = Eigen::Vector2d::Constant(1.0),
                double _force = 1e-5)
    : linear_momentum_integral(_linear_momentum_integral), linear_momentum(_linear_momentum),
      angular_momentum(_angular_momentum), force(_force)
    {
    }
 
    Eigen::VectorXd inputWeight(int total_input_dim) const;
 
    Eigen::VectorXd outputWeight(size_t seq_len) const;
  };
 
  class Model : public _StateSpaceModel
  {
  public:
    Model(double mass, const MotionParam & motion_param, int output_dim = 0);
 
  public:
    MotionParam motion_param_;
  };
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  LinearMpcXY(double mass,
              double horizon_dt,
              int horizon_steps,
              const WeightParam & weight_param = WeightParam(),
              QpSolverCollection::QpSolverType qp_solver_type = QpSolverCollection::QpSolverType::Any);
 
  Eigen::VectorXd planOnce(const std::function<MotionParam(double)> & motion_param_func,
                           const std::function<RefData(double)> & ref_data_func,
                           const InitialParam & initial_param,
                           double current_time);
 
protected:
  Eigen::VectorXd procOnce(const std::vector<std::shared_ptr<_StateSpaceModel>> & model_list,
                           const StateDimVector & current_x,
                           const Eigen::VectorXd & ref_output_seq);
 
public:
  double mass_ = 0;
 
  double horizon_dt_ = 0;
 
  int horizon_steps_ = 0;
 
  WeightParam weight_param_;
 
  std::shared_ptr<QpSolverCollection::QpSolver> qp_solver_;
 
  QpSolverCollection::QpCoeff qp_coeff_;
 
  std::pair<double, double> force_range_;
};
} // namespace CCC