PreviewControlCentroidal.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <SpaceVecAlg/SpaceVecAlg>
 
#include <mc_rtc/Configuration.h>
 
#include <CCC/PreviewControl.h>
 
namespace ForceColl
{
class Contact;
}
 
namespace CCC
{
class PreviewControlCentroidal1d : public PreviewControl<3, 1, 2>
{
  friend class PreviewControlCentroidal;
 
public:
  using RefData = Eigen::Vector2d;
 
  using InitialParam = PreviewControl<3, 1, 2>::StateDimVector;
 
  struct WeightParam
  {
    double pos;
 
    double wrench;
 
    double jerk;
 
    WeightParam(double _pos = 2e2, double _wrench = 5e-4, double _jerk = 1e-8) : pos(_pos), wrench(_wrench), jerk(_jerk)
    {
    }
 
    PreviewControl<3, 1, 2>::WeightParam toPreviewControlWeightParam() const;
  };
 
  class CentroidalModel1d : public StateSpaceModel<3, 1, 2>
  {
  public:
    CentroidalModel1d(double inertia_param);
  };
 
public:
  PreviewControlCentroidal1d(double inertia_param,
                             double horizon_duration,
                             double horizon_dt,
                             const WeightParam & weight_param = WeightParam())
  : PreviewControl<3, 1, 2>(std::make_shared<CentroidalModel1d>(inertia_param),
                            horizon_duration,
                            horizon_dt,
                            weight_param.toPreviewControlWeightParam())
  {
  }
 
protected:
  double procOnce(const Eigen::VectorXd & ref_output_seq,
                  const InitialParam & initial_param,
                  double current_time,
                  double control_dt) const;
};
 
class PreviewControlCentroidal
{
public:
  struct MotionParam
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    std::vector<std::shared_ptr<ForceColl::Contact>> contact_list;
  };
 
  struct RefData
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    sva::MotionVecd pos = sva::MotionVecd::Zero();
 
    sva::ForceVecd wrench = sva::ForceVecd::Zero();
  };
 
  struct InitialParam
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    sva::MotionVecd pos = sva::MotionVecd::Zero();
 
    sva::MotionVecd vel = sva::MotionVecd::Zero();
 
    sva::MotionVecd acc = sva::MotionVecd::Zero();
 
    PreviewControlCentroidal1d::InitialParam toInitialParam1d(int idx) const;
  };
 
  struct WeightParam
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    sva::MotionVecd pos;
 
    sva::ForceVecd wrench;
 
    sva::MotionVecd jerk;
 
    mc_rtc::Configuration wrench_dist_config;
 
    WeightParam(const sva::MotionVecd & _pos = sva::MotionVecd(Eigen::Vector3d(1e2, 1e2, 1e2),
                                                               Eigen::Vector3d(2e2, 2e2, 2e2)),
                const sva::ForceVecd & _wrench = sva::ForceVecd(Eigen::Vector3d(5e-3, 5e-3, 5e-3),
                                                                Eigen::Vector3d(5e-4, 5e-4, 5e-4)),
                const sva::MotionVecd & _jerk = sva::MotionVecd(Eigen::Vector3d::Constant(1e-8),
                                                                Eigen::Vector3d::Constant(1e-8)),
                const mc_rtc::Configuration & _wrench_dist_config = {})
    : pos(_pos), wrench(_wrench), jerk(_jerk), wrench_dist_config(_wrench_dist_config)
    {
    }
 
    PreviewControlCentroidal1d::WeightParam toWeightParam1d(int idx) const;
  };
 
public:
  PreviewControlCentroidal(double mass,
                           const Eigen::Vector3d & moment_of_inertia,
                           double horizon_duration,
                           double horizon_dt,
                           const WeightParam & weight_param = WeightParam());
 
  sva::ForceVecd planOnce(const MotionParam & motion_param,
                          const std::function<RefData(double)> & ref_data_func,
                          const InitialParam & initial_param,
                          double current_time,
                          double control_dt = -1) const;
 
public:
  std::array<std::shared_ptr<PreviewControlCentroidal1d>, 6> preview_control_1d_;
 
  mc_rtc::Configuration wrench_dist_config_;
 
protected:
  double mass_;
};
} // namespace CCC