StepMpc.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <optional>
 
#include <CCC/VariantSequentialExtension.h>
 
namespace CCC
{
class StepMpc1d
{
  friend class StepMpc;
 
public:
  using _StateSpaceModel = StateSpaceModel<2, Eigen::Dynamic, Eigen::Dynamic>;
 
  class StepModel : public _StateSpaceModel
  {
  public:
    StepModel(double com_height, double step_duration);
  };
 
  struct RefData
  {
    struct Element
    {
      bool is_single_support = true;
 
      double zmp = 0;
 
      double end_time = 0;
    };
 
    std::vector<Element> element_list;
  };
 
  struct PlannedData
  {
    double current_zmp = 0;
 
    std::optional<double> next_foot_zmp;
  };
 
  using InitialParam = Eigen::Vector2d;
 
  struct WeightParam
  {
    double free_zmp;
 
    double fixed_zmp;
 
    double double_support;
 
    double pos;
 
    double vel;
 
    double capture_point_abs;
 
    double capture_point_rel;
 
    WeightParam(double _free_zmp = 1e-2,
                double _fixed_zmp = 1e0,
                double _double_support = 1e0,
                double _pos = 0.0,
                double _vel = 0.0,
                double _capture_point_abs = 1e1,
                double _capture_point_rel = 1e1)
    : free_zmp(_free_zmp), fixed_zmp(_fixed_zmp), double_support(_double_support), pos(_pos), vel(_vel),
      capture_point_abs(_capture_point_abs), capture_point_rel(_capture_point_rel)
    {
    }
  };
 
public:
  StepMpc1d(double com_height, const WeightParam & weight_param = WeightParam())
  : com_height_(com_height), weight_param_(weight_param)
  {
  }
 
  PlannedData planOnce(const RefData & ref_data, const InitialParam & initial_param, double current_time);
 
protected:
  double com_height_ = 0;
 
  WeightParam weight_param_;
 
  std::shared_ptr<VariantSequentialExtension<2>> seq_ext_;
};
 
class StepMpc
{
public:
  struct RefData
  {
    struct Element
    {
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
      bool is_single_support = true;
 
      Eigen::Vector2d zmp = Eigen::Vector2d::Zero();
 
      double end_time = 0;
    };
 
    std::vector<Element> element_list;
  };
 
  struct PlannedData
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector2d current_zmp = Eigen::Vector2d::Zero();
 
    std::optional<Eigen::Vector2d> next_foot_zmp;
  };
 
  struct InitialParam
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector2d pos = Eigen::Vector2d::Zero();
 
    Eigen::Vector2d vel = Eigen::Vector2d::Zero();
  };
 
public:
  StepMpc(double com_height, const StepMpc1d::WeightParam & weight_param = StepMpc1d::WeightParam())
  : mpc_1d_(std::make_shared<StepMpc1d>(com_height, weight_param))
  {
  }
 
  PlannedData planOnce(const RefData & ref_data, const InitialParam & initial_param, double current_time);
 
protected:
  std::shared_ptr<StepMpc1d> mpc_1d_;
};
} // namespace CCC