SingularPreviewControlZmp.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <functional>
#include <memory>
 
#include <Eigen/Dense>
 
#include <CCC/Constants.h>
 
namespace CCC
{
class SingularPreviewControlZmp1d
{
  friend class SingularPreviewControlZmp;
 
public:
  struct InitialParam
  {
    double pos = 0;
 
    double vel = 0;
 
    double planned_zmp = 0;
  };
 
public:
  SingularPreviewControlZmp1d(double com_height, double horizon_duration, double horizon_dt)
  : horizon_dt_(horizon_dt), horizon_steps_(static_cast<int>(std::ceil(horizon_duration / horizon_dt))),
    omega_(std::sqrt(constants::g / com_height))
  {
  }
 
  double planOnce(const std::function<double(double)> & ref_zmp_func,
                  const InitialParam & initial_param,
                  double current_time,
                  double control_dt = -1) const;
 
protected:
  double procOnce(const Eigen::VectorXd & ref_zmp_seq,
                  const InitialParam & initial_param,
                  double current_time,
                  double control_dt) const;
 
protected:
  double horizon_dt_ = 0;
 
  int horizon_steps_ = -1;
 
  double omega_ = 0;
};
 
class SingularPreviewControlZmp
{
public:
  struct InitialParam
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector2d pos = Eigen::Vector2d::Zero();
 
    Eigen::Vector2d vel = Eigen::Vector2d::Zero();
 
    Eigen::Vector2d planned_zmp = Eigen::Vector2d::Zero();
  };
 
public:
  SingularPreviewControlZmp(double com_height, double horizon_duration, double horizon_dt)
  : spc_1d_(std::make_shared<SingularPreviewControlZmp1d>(com_height, horizon_duration, horizon_dt))
  {
  }
 
  Eigen::Vector2d planOnce(const std::function<Eigen::Vector2d(double)> & ref_zmp_func,
                           const InitialParam & initial_param,
                           double current_time,
                           double control_dt = -1) const;
 
public:
  std::shared_ptr<SingularPreviewControlZmp1d> spc_1d_;
};
} // namespace CCC