StabilizerTask.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 *
 * This file is inspired by Stephane's Caron original implementation as part of
 * lipm_walking_controller <https://github.com/stephane-caron/lipm_walking_controller>
 */
 
#pragma once
 
#include <mc_filter/ExponentialMovingAverage.h>
#include <mc_filter/LeakyIntegrator.h>
#include <mc_filter/LowPass.h>
#include <mc_filter/LowPassCompose.h>
#include <mc_filter/StationaryOffset.h>
#include <mc_rbdyn/lipm_stabilizer/StabilizerConfiguration.h>
#include <mc_rtc/deprecated.h>
#include <mc_tasks/CoMTask.h>
#include <mc_tasks/CoPTask.h>
#include <mc_tasks/MetaTask.h>
#include <mc_tasks/OrientationTask.h>
#include <mc_tasks/lipm_stabilizer/Contact.h>
#include <mc_tasks/lipm_stabilizer/ZMPCC.h>
 
#include <state-observation/dynamics-estimators/lipm-dcm-estimator.hpp>
 
#include <Eigen/QR>
#include <eigen-quadprog/QuadProg.h>
 
namespace mc_tasks
{
 
namespace lipm_stabilizer
{
 
using ::mc_filter::utils::clamp;
using ZMPCCConfiguration = mc_rbdyn::lipm_stabilizer::ZMPCCConfiguration;
using StabilizerConfiguration = mc_rbdyn::lipm_stabilizer::StabilizerConfiguration;
using FDQPWeights = mc_rbdyn::lipm_stabilizer::FDQPWeights;
using SafetyThresholds = mc_rbdyn::lipm_stabilizer::SafetyThresholds;
using DCMBiasEstimatorConfiguration = mc_rbdyn::lipm_stabilizer::DCMBiasEstimatorConfiguration;
using ExternalWrenchConfiguration = mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration;
 
struct MC_TASKS_DLLAPI StabilizerTask : public MetaTask
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  struct ExternalWrench
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    sva::ForceVecd target;
    sva::ForceVecd measured;
    sva::MotionVecd gain;
    std::string surfaceName;
  };
 
  StabilizerTask(const mc_rbdyn::Robots & robots,
                 const mc_rbdyn::Robots & realRobots,
                 unsigned int robotIndex,
                 const std::string & leftSurface,
                 const std::string & rightSurface,
                 const std::string & torsoBodyName,
                 double dt);
 
  StabilizerTask(const mc_rbdyn::Robots & robots,
                 const mc_rbdyn::Robots & realRobots,
                 unsigned int robotIndex,
                 double dt);
 
  inline void name(const std::string & name) override
  {
    name_ = name;
 
    // Rename the tasks managed by the stabilizer
    // Doing so helps making the logs more consistent, and having a fixed name
    // allows for predifined custom plots in the log ui.
    const auto n = name_ + "_Tasks";
    comTask->name(n + "_com");
    footTasks.at(ContactState::Left)->name(n + "_cop_left");
    footTasks.at(ContactState::Right)->name(n + "_cop_right");
    pelvisTask->name(n + "_pelvis");
    torsoTask->name(n + "_torso");
  }
 
  using MetaTask::name;
 
  void reset() override;
 
  Eigen::VectorXd eval() const override;
 
  Eigen::VectorXd speed() const override;
 
  void enable();
 
  void disable();
 
  void configure(const StabilizerConfiguration & config);
 
  void commitConfig();
 
  void load(mc_solver::QPSolver &, const mc_rtc::Configuration & config) override;
 
  const StabilizerConfiguration & config() const;
 
  const StabilizerConfiguration & commitedConfig() const;
 
  void reconfigure();
 
  void run();
 
  void setContacts(const ContactDescriptionVector & contacts);
 
  void setContacts(const std::vector<std::pair<ContactState, sva::PTransformd>> & contacts);
 
  void setContacts(const std::vector<ContactState> & contacts);
 
  inline const sva::PTransformd & contactAnklePose(ContactState s) const { return contacts_.at(s).anklePose(); }
 
  inline const std::string & footSurface(ContactState s) const { return footTasks.at(s)->surface(); }
 
  inline double leftFootRatio() const noexcept { return leftFootRatio_; }
 
  sva::PTransformd anchorFrame(const mc_rbdyn::Robot & robot) const;
 
  void staticTarget(const Eigen::Vector3d & com, double zmpHeight = 0);
 
  void target(const Eigen::Vector3d & com,
              const Eigen::Vector3d & comd,
              const Eigen::Vector3d & comdd,
              const Eigen::Vector3d & zmp,
              const Eigen::Vector3d & zmpd = Eigen::Vector3d::Zero());
 
  inline const Eigen::Vector3d & targetCoMRaw() const noexcept { return comTargetRaw_; }
 
  inline const Eigen::Vector3d & targetCoM() const noexcept { return comTargetRaw_; }
 
  /* Return the target CoM velocity provided by \ref stabilizer_target "target()" */
  inline const Eigen::Vector3d & targetCoMVelocity() const noexcept { return comdTarget_; }
 
  /* Return the target CoM acceleration provided by \ref stabilizer_target "target()" */
  inline const Eigen::Vector3d & targetCoMAcceleration() const noexcept { return comddTarget_; }
 
  /* Return the target ZMP provided by \ref stabilizer_target "target()" */
  inline const Eigen::Vector3d & targetZMP() const noexcept { return zmpTarget_; }
 
  /* Return the target ZMP velocity by \ref stabilizer_target "target()" */
  inline const Eigen::Vector3d & targetZMPVelocity() const noexcept { return zmpdTarget_; }
 
  /* Return the current support foot */
  inline ContactState supportFoot() const noexcept { return supportFoot_; }
 
  /* Set the current support foot */
  inline void supportFoot(const ContactState & foot) noexcept { supportFoot_ = foot; }
 
  inline void horizonReference(const std::vector<Eigen::Vector2d> & ref, const double delta) noexcept
  {
    horizonZmpRef_ = ref;
    horizonDelta_ = delta;
    horizonCoPDistribution_ = true;
    horizonRefUpdated_ = true;
  }
 
  void setExternalWrenches(const std::vector<std::string> & surfaceNames,
                           const std::vector<sva::ForceVecd> & targetWrenches,
                           const std::vector<sva::MotionVecd> & gains);
 
  /* Return the current external wrenches targets
   *
   * \see \ref set_external_wrenches "setExternalWrenches()"
   **/
  inline const std::vector<ExternalWrench> & externalWrenches() const noexcept { return extWrenches_; }
 
  /*
   * Return the ZMP from the wrenches distribution
   **/
  inline const Eigen::Vector3d & distribZMP() const noexcept { return distribZMP_; }
 
  inline const Eigen::Vector3d & measuredDCM() noexcept { return measuredDCM_; }
 
  inline const Eigen::Vector2d biasDCM() noexcept
  {
    if(c_.dcmBias.withDCMBias) { return dcmEstimator_.getBias(); }
    return Eigen::Vector2d::Zero();
  }
 
  inline Eigen::Vector2d filteredDCM() const noexcept
  {
    if(c_.dcmBias.withDCMBias) { return dcmEstimator_.getUnbiasedDCM(); }
    return measuredDCM_.segment(0, 2);
  }
 
  inline const Eigen::Vector3d & measuredZMP() noexcept { return measuredZMP_; }
 
  inline const Eigen::Vector3d & measuredCoM() noexcept { return measuredCoM_; }
 
  inline const Eigen::Vector3d & measuredCoMd() noexcept { return measuredCoMd_; }
 
  inline const Eigen::Vector3d & measuredFilteredNetForces() const noexcept { return fSumFilter_.eval(); }
 
  inline const Eigen::Vector3d & comOffsetTarget() noexcept { return comOffsetTarget_; }
 
  inline const Eigen::Vector3d & comOffsetMeasured() const noexcept { return comOffsetMeasured_; }
 
  inline double zmpCoeffMeasured() const noexcept { return zmpCoefMeasured_; }
 
  inline bool inContact(ContactState state) const noexcept { return contacts_.count(state); }
 
  inline bool inDoubleSupport() const noexcept { return contacts_.size() == 2; }
 
  inline const mc_rbdyn::Robot & robot() const noexcept { return robots_.robot(robotIndex_); }
 
  inline const mc_rbdyn::Robot & realRobot() const noexcept { return realRobots_.robot(robotIndex_); }
 
  inline void torsoPitch(double pitch) noexcept { c_.torsoPitch = pitch; }
 
  inline double omega() const { return omega_; }
 
  inline void torsoWeight(double weight) noexcept
  {
    c_.torsoWeight = weight;
    torsoTask->weight(c_.torsoWeight);
  }
 
  inline void torsoStiffness(double stiffness) noexcept
  {
    c_.torsoStiffness = stiffness;
    torsoTask->stiffness(stiffness);
  }
 
  inline void pelvisWeight(double weight) noexcept
  {
    c_.pelvisWeight = weight;
    pelvisTask->weight(c_.pelvisWeight);
  }
 
  inline void pelvisStiffness(double stiffness) noexcept
  {
    c_.pelvisStiffness = stiffness;
    pelvisTask->stiffness(stiffness);
  }
 
  inline void dcmGains(double p, double i, double d) noexcept
  { dcmGains(Eigen::Vector2d::Constant(p), Eigen::Vector2d::Constant(i), Eigen::Vector2d::Constant(d)); }
 
  inline void dcmGains(const Eigen::Vector2d & p, const Eigen::Vector2d & i, const Eigen::Vector2d & d) noexcept
  {
    c_.dcmPropGain = clamp(p, 0., c_.safetyThresholds.MAX_DCM_P_GAIN);
    c_.dcmIntegralGain = clamp(i, 0., c_.safetyThresholds.MAX_DCM_I_GAIN);
    c_.dcmDerivGain = clamp(d, 0., c_.safetyThresholds.MAX_DCM_D_GAIN);
  }
 
  inline void dcmIntegratorTimeConstant(double dcmIntegratorTimeConstant) noexcept
  {
    c_.dcmIntegratorTimeConstant = dcmIntegratorTimeConstant;
    dcmIntegrator_.timeConstant(dcmIntegratorTimeConstant);
  }
 
  MC_RTC_DEPRECATED inline void dcmDerivatorTimeConstant(double T) noexcept { dcmDerivatorCutoffPeriod(T); }
 
  inline void dcmDerivatorCutoffPeriod(double T) noexcept
  {
    c_.dcmDerivatorTimeConstant = T;
    dcmDerivator_.cutoffPeriod(T);
  }
 
  inline void extWrenchSumLowPassCutoffPeriod(double cutoffPeriod) noexcept
  {
    c_.extWrench.extWrenchSumLowPassCutoffPeriod = cutoffPeriod;
    extWrenchSumLowPass_.cutoffPeriod(cutoffPeriod);
  }
 
  inline void comOffsetLowPassCutoffPeriod(double cutoffPeriod) noexcept
  {
    c_.extWrench.comOffsetLowPassCutoffPeriod = cutoffPeriod;
    comOffsetLowPass_.cutoffPeriod(cutoffPeriod);
  }
 
  inline void comOffsetLowPassCoMCutoffPeriod(double cutoffPeriod) noexcept
  {
    c_.extWrench.comOffsetLowPassCoMCutoffPeriod = cutoffPeriod;
    comOffsetLowPassCoM_.cutoffPeriod(cutoffPeriod);
  }
 
  inline void comOffsetDerivatorTimeConstant(double timeConstant) noexcept
  {
    c_.extWrench.comOffsetDerivatorTimeConstant = timeConstant;
    comOffsetDerivator_.timeConstant(timeConstant);
  }
 
  inline void comWeight(double weight) noexcept
  {
    c_.comWeight = weight;
    comTask->weight(weight);
  }
 
  inline void comStiffness(const Eigen::Vector3d & stiffness) noexcept
  {
    c_.comStiffness = stiffness;
    comTask->stiffness(stiffness);
  }
 
  inline void contactWeight(double weight) noexcept
  {
    c_.contactWeight = weight;
    for(auto footT : contactTasks) { footT->weight(c_.contactWeight); }
  }
 
  inline void contactStiffness(const sva::MotionVecd & stiffness) noexcept
  {
    c_.contactStiffness = stiffness;
    for(auto contactT : contactTasks) { contactT->stiffness(stiffness); }
  }
 
  inline void contactDamping(const sva::MotionVecd & damping) noexcept
  {
    c_.contactDamping = damping;
    for(auto contactT : contactTasks) { contactT->damping(damping); }
  }
 
  inline void copAdmittance(const Eigen::Vector2d & copAdmittance) noexcept
  {
    c_.copAdmittance = clamp(copAdmittance, 0., c_.safetyThresholds.MAX_COP_ADMITTANCE);
    for(auto contactT : contactTasks) { contactT->admittance(contactAdmittance()); }
  }
 
  inline void copMaxVel(const sva::MotionVecd & copMaxVel) noexcept
  {
    c_.copMaxVel = copMaxVel;
    for(const auto & footTask : footTasks)
    {
      footTask.second->maxLinearVel(copMaxVel.linear());
      footTask.second->maxAngularVel(copMaxVel.angular());
    }
  }
 
  /* Set the gain of the low-pass velocity filter of the cop tasks */
  inline void copVelFilterGain(double gain) noexcept
  {
    c_.copVelFilterGain = mc_filter::utils::clamp(gain, 0, 1);
    for(auto & ft : footTasks) { ft.second->velFilterGain(gain); }
  }
 
  /* Get the gain of the low-pass velocity filter of the cop tasks */
  inline double copVelFilterGain() const noexcept { return c_.copVelFilterGain; }
 
  inline void vdcFrequency(double freq) noexcept { c_.vdcFrequency = clamp(freq, 0., 10.); }
 
  inline void vdcStiffness(double stiffness) noexcept { c_.vdcStiffness = clamp(stiffness, 0., 1e4); }
 
  inline void dfAdmittance(Eigen::Vector3d dfAdmittance) noexcept
  { c_.dfAdmittance = clamp(dfAdmittance, 0., c_.safetyThresholds.MAX_DF_ADMITTANCE); }
 
  inline void dfDamping(Eigen::Vector3d dfDamping) noexcept
  { c_.dfDamping = clamp(dfDamping, 0., c_.safetyThresholds.MAX_DF_DAMPING); }
 
  inline void fdqpWeights(const FDQPWeights & fdqp) noexcept { c_.fdqpWeights = fdqp; }
 
  inline void safetyThresholds(const SafetyThresholds & thresholds) noexcept
  {
    c_.safetyThresholds = thresholds;
    c_.clampGains();
    // only requried because we want to apply the new gains immediately
    copAdmittance(c_.copAdmittance);
  }
 
  inline void dcmBiasEstimatorConfiguration(const DCMBiasEstimatorConfiguration & biasConfig) noexcept
  {
    auto & bc = c_.dcmBias;
    bc = biasConfig;
    dcmEstimator_.setBiasDriftPerSecond(bc.biasDriftPerSecondStd);
    dcmEstimator_.setDcmMeasureErrorStd(bc.dcmMeasureErrorStd);
    dcmEstimator_.setZmpMeasureErrorStd(bc.zmpMeasureErrorStd);
    dcmEstimator_.setBiasLimit(bc.biasLimit);
  }
 
  inline const DCMBiasEstimatorConfiguration & dcmBiasEstimatorConfiguration() const noexcept { return c_.dcmBias; }
 
  inline void externalWrenchConfiguration(const ExternalWrenchConfiguration & extWrenchConfig) noexcept
  {
    c_.extWrench = extWrenchConfig;
    extWrenchSumLowPass_.cutoffPeriod(c_.extWrench.extWrenchSumLowPassCutoffPeriod);
    comOffsetLowPass_.cutoffPeriod(c_.extWrench.comOffsetLowPassCutoffPeriod);
    comOffsetLowPassCoM_.cutoffPeriod(c_.extWrench.comOffsetLowPassCoMCutoffPeriod);
    comOffsetDerivator_.timeConstant(c_.extWrench.comOffsetDerivatorTimeConstant);
  }
 
  inline const ExternalWrenchConfiguration & externalWrenchConfiguration() const noexcept { return c_.extWrench; }
 
private:
  void dimWeight(const Eigen::VectorXd & dimW) override;
  Eigen::VectorXd dimWeight() const override;
 
  void selectActiveJoints(mc_solver::QPSolver & solver,
                          const std::vector<std::string> & activeJointsName,
                          const std::map<std::string, std::vector<std::array<int, 2>>> & activeDofs = {}) override;
 
  void selectUnactiveJoints(mc_solver::QPSolver & solver,
                            const std::vector<std::string> & unactiveJointsName,
                            const std::map<std::string, std::vector<std::array<int, 2>>> & unactiveDofs = {}) override;
 
  void resetJointsSelector(mc_solver::QPSolver & solver) override;
 
  void addContact(ContactState contactState, const internal::Contact & contact);
 
  void checkInTheAir();
 
  void computeLeftFootRatio();
 
  void updateState(const Eigen::Vector3d & com, const Eigen::Vector3d & comd, const Eigen::Vector3d & comdd);
 
  void updateContacts(mc_solver::QPSolver & solver);
 
  sva::ForceVecd computeDesiredWrench();
 
  void distributeWrench(const sva::ForceVecd & desiredWrench);
 
  void distributeCoPonHorizon(const std::vector<Eigen::Vector2d> & zmp_ref, double delta);
 
  void computeCoPonHorizon(const std::vector<Eigen::Vector2d> & zmp_ref, const double delta, const double t_delay);
 
  void saturateWrench(const sva::ForceVecd & desiredWrench,
                      std::shared_ptr<mc_tasks::force::CoPTask> & footTask,
                      const internal::Contact & contact);
 
  void setSupportFootGains();
 
  void updateCoMTaskZMPCC();
 
  void updateFootForceDifferenceControl();
 
  void updateZMPFrame();
 
  inline sva::ForceVecd contactAdmittance() const noexcept
  { return {{c_.copAdmittance.y(), c_.copAdmittance.x(), 0.}, {0., 0., 0.}}; }
 
  inline void zmpcc(const ZMPCCConfiguration & zmpccConfig) noexcept
  {
    c_.zmpcc = zmpccConfig;
    zmpcc_.configure(zmpccConfig);
  }
 
  template<sva::ForceVecd ExternalWrench::* TargetOrMeasured>
  void computeWrenchOffsetAndCoefficient(const mc_rbdyn::Robot & robot,
                                         Eigen::Vector3d & offset_gamma,
                                         double & coef_kappa) const;
 
  template<sva::ForceVecd ExternalWrench::* TargetOrMeasured>
  sva::ForceVecd computeExternalWrenchSum(const mc_rbdyn::Robot & robot, const Eigen::Vector3d & com) const;
 
  void computeExternalContact(const mc_rbdyn::Robot & robot,
                              const std::string & surfaceName,
                              const sva::ForceVecd & surfaceWrench,
                              Eigen::Vector3d & pos,
                              Eigen::Vector3d & force,
                              Eigen::Vector3d & moment) const;
 
  /* Task-related properties */
protected:
  void addToSolver(mc_solver::QPSolver & solver) override;
  void removeFromSolver(mc_solver::QPSolver & solver) override;
  void removeFromGUI(mc_rtc::gui::StateBuilder &) override;
  void update(mc_solver::QPSolver &) override;
 
  void addToLogger(mc_rtc::Logger &) override;
  void removeFromLogger(mc_rtc::Logger &) override;
  void addToGUI(mc_rtc::gui::StateBuilder &) override;
 
  void configure_(mc_solver::QPSolver & solver);
  void disable_();
 
  void checkConfiguration(const StabilizerConfiguration & config);
 
protected:
  struct EnumClassHash
  {
    template<typename T>
    std::size_t operator()(T t) const
    { return static_cast<std::size_t>(t); }
  };
  std::unordered_map<ContactState,
                     internal::Contact,
                     EnumClassHash,
                     std::equal_to<ContactState>,
                     Eigen::aligned_allocator<std::pair<const ContactState, internal::Contact>>>
      contacts_;
  std::vector<ContactState> addContacts_; 
  std::unordered_map<ContactState, std::shared_ptr<mc_tasks::force::CoPTask>, EnumClassHash> footTasks;
  std::vector<std::shared_ptr<mc_tasks::force::CoPTask>> contactTasks; 
  std::vector<std::string> contactSensors; 
  std::vector<std::vector<Eigen::Vector3d>> supportPolygons_; 
  Eigen::Vector2d supportMin_ = Eigen::Vector2d::Zero();
  Eigen::Vector2d supportMax_ = Eigen::Vector2d::Zero();
  std::shared_ptr<mc_tasks::CoMTask> comTask;
  std::shared_ptr<mc_tasks::OrientationTask> pelvisTask; 
  std::shared_ptr<mc_tasks::OrientationTask> torsoTask; 
  const mc_rbdyn::Robots & robots_;
  const mc_rbdyn::Robots & realRobots_;
  unsigned int robotIndex_;
 
  Eigen::Vector3d comTargetRaw_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d comTarget_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d comdTarget_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d comddTarget_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d zmpTarget_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d zmpdTarget_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d dcmTarget_ = Eigen::Vector3d::Zero();
  double omega_ = 3.4;
 
  double t_ = 0.; 
protected:
  StabilizerConfiguration defaultConfig_;
  StabilizerConfiguration lastConfig_;
  StabilizerConfiguration disableConfig_;
  StabilizerConfiguration c_;
  bool reconfigure_ = true;
  bool enabled_ = true; 
  Eigen::QuadProgDense qpSolver_; 
  Eigen::Vector3d dcmAverageError_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d dcmError_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d dcmVelError_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d measuredCoM_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d measuredCoMd_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d measuredCoMdd_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d measuredZMP_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d measuredDCM_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d measuredDCMUnbiased_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d measuredCoMUnbiased_ = Eigen::Vector3d::Zero(); 
  sva::ForceVecd measuredNetWrench_ = sva::ForceVecd::Zero();
 
  bool zmpccOnlyDS_ = true; 
  ZMPCC zmpcc_; 
  stateObservation::LipmDcmEstimator dcmEstimator_;
  bool dcmEstimatorNeedsReset_ = true;
 
  std::vector<ExternalWrench> extWrenches_;
  sva::ForceVecd extWrenchSumTarget_ = sva::ForceVecd::Zero(); 
  sva::ForceVecd extWrenchSumMeasured_ = sva::ForceVecd::Zero(); 
  Eigen::Vector3d comOffsetTarget_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d comOffsetMeasured_ = Eigen::Vector3d::Zero(); 
  double zmpCoefTarget_ = 1; 
  double zmpCoefMeasured_ = 1; 
  Eigen::Vector3d comOffsetErr_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d comOffsetErrCoM_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d comOffsetErrZMP_ = Eigen::Vector3d::Zero(); 
  mc_filter::LowPass<sva::ForceVecd>
      extWrenchSumLowPass_; 
  mc_filter::LowPass<Eigen::Vector3d> comOffsetLowPass_; 
  mc_filter::LowPass<Eigen::Vector3d>
      comOffsetLowPassCoM_; 
  mc_filter::StationaryOffset<Eigen::Vector3d> comOffsetDerivator_; 
  mc_filter::ExponentialMovingAverage<Eigen::Vector3d> dcmIntegrator_;
  mc_filter::LowPassCompose<Eigen::Vector3d> dcmDerivator_;
  bool inTheAir_ = false; 
  bool wasInTheAir_ = false; 
  bool wasEnabled_ = true; 
  Eigen::Vector3d dfForceError_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d dfError_ = Eigen::Vector3d::Zero(); 
  double dt_ = 0.005; 
  double leftFootRatio_ = 0.5; 
  double mass_ = 38.; 
  double runTime_ = 0.;
  double vdcHeightError_ = 0; 
  sva::ForceVecd desiredWrench_ = sva::ForceVecd::Zero(); 
  sva::ForceVecd distribWrench_ = sva::ForceVecd::Zero(); 
  Eigen::Vector3d distribZMP_ =
      Eigen::Vector3d::Zero(); 
  sva::PTransformd zmpFrame_ = sva::PTransformd::Identity(); 
  // CoP distribution over an horizon
  //{
  std::vector<Eigen::Vector2d> horizonZmpRef_; 
  double horizonDelta_ = 0.05; 
  bool horizonCoPDistribution_ = false;
  bool horizonRefUpdated_ = false;
  Eigen::Vector2d modeledCoPLeft_ = Eigen::Vector2d::Zero(); 
  Eigen::Vector2d modeledCoPRight_ = Eigen::Vector2d::Zero(); 
  Eigen::Vector2d delayedTargetCoPLeft_ = Eigen::Vector2d::Zero(); 
  Eigen::Vector2d delayedTargetCoPRight_ = Eigen::Vector2d::Zero(); 
  double delayedTargetFzLeft_ = 0; 
  double delayedTargetFzRight_ = 0; 
  double tComputation_ = 0.; 
  double modeledFzRight_ = 0.; 
  double modeledFzLeft_ = 0.; 
  double desiredFzLeft_ = 0.; 
  double desiredFzRight_ = 0.; 
  Eigen::Vector2d QPCoPLeft_ =
      Eigen::Vector2d::Zero(); 
  Eigen::Vector2d QPCoPRight_ =
      Eigen::Vector2d::Zero(); 
  Eigen::Vector2d distribCheck_ = Eigen::Vector2d::Zero();
  mc_filter::LowPass<Eigen::Vector3d> fSumFilter_; 
  ContactState supportFoot_ = ContactState::Left; 
  //}
};
 
extern template void StabilizerTask::computeWrenchOffsetAndCoefficient<&StabilizerTask::ExternalWrench::target>(
    const mc_rbdyn::Robot & robot,
    Eigen::Vector3d & offset_gamma,
    double & coef_kappa) const;
extern template void StabilizerTask::computeWrenchOffsetAndCoefficient<&StabilizerTask::ExternalWrench::measured>(
    const mc_rbdyn::Robot & robot,
    Eigen::Vector3d & offset_gamma,
    double & coef_kappa) const;
 
extern template sva::ForceVecd StabilizerTask::computeExternalWrenchSum<&StabilizerTask::ExternalWrench::target>(
    const mc_rbdyn::Robot &,
    const Eigen::Vector3d &) const;
extern template sva::ForceVecd StabilizerTask::computeExternalWrenchSum<&StabilizerTask::ExternalWrench::measured>(
    const mc_rbdyn::Robot &,
    const Eigen::Vector3d &) const;
 
} // namespace lipm_stabilizer
} // namespace mc_tasks