PreviewControlZmp.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <CCC/CommonModels.h>
#include <CCC/PreviewControl.h>
 
namespace CCC
{
class PreviewControlZmp1d : public PreviewControl<3, 1, 1>
{
  friend class PreviewControlZmp;
 
public:
  using InitialParam = PreviewControl<3, 1, 1>::StateDimVector;
 
  struct WeightParam
  {
    double zmp;
 
    double com_jerk;
 
    WeightParam(double _zmp = 1.0, double _com_jerk = 1e-8) : zmp(_zmp), com_jerk(_com_jerk) {}
 
    PreviewControl<3, 1, 1>::WeightParam toPreviewControlWeightParam() const;
  };
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  PreviewControlZmp1d(double com_height,
                      double horizon_duration,
                      double horizon_dt,
                      const WeightParam & weight_param = WeightParam())
  : PreviewControl<3, 1, 1>(std::make_shared<ComZmpModelJerkInput>(com_height),
                            horizon_duration,
                            horizon_dt,
                            weight_param.toPreviewControlWeightParam())
  {
  }
 
  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;
};
 
class PreviewControlZmp
{
public:
  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:
  PreviewControlZmp(double com_height,
                    double horizon_duration,
                    double horizon_dt,
                    const PreviewControlZmp1d::WeightParam & weight_param = PreviewControlZmp1d::WeightParam())
  : preview_control_1d_(std::make_shared<PreviewControlZmp1d>(com_height, horizon_duration, horizon_dt, weight_param))
  {
  }
 
  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<PreviewControlZmp1d> preview_control_1d_;
};
} // namespace CCC