StabilizerConfiguration.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_rbdyn/Gains.h>
#include <mc_rbdyn/api.h>
#include <mc_rbdyn/lipm_stabilizer/ZMPCCConfiguration.h>
 
#include <mc_filter/utils/clamp.h>
 
#include <mc_rtc/Configuration.h>
#include <mc_rtc/logging.h>
 
namespace mc_rbdyn
{
namespace lipm_stabilizer
{
 
struct MC_RBDYN_DLLAPI FDQPWeights
{
  FDQPWeights() : ankleTorqueSqrt(std::sqrt(100)), netWrenchSqrt(std::sqrt(10000)), pressureSqrt(std::sqrt(1)) {}
  /* All weights must be strictly positive */
  FDQPWeights(double netWrench, double ankleTorque, double pressure)
  : ankleTorqueSqrt(std::sqrt(ankleTorque)), netWrenchSqrt(std::sqrt(netWrench)), pressureSqrt(std::sqrt(pressure))
  {
  }
  double ankleTorqueSqrt;
  double netWrenchSqrt;
  double pressureSqrt;
 
  void load(const mc_rtc::Configuration & config)
  {
    if(config.has("ankle_torque")) { ankleTorqueSqrt = std::sqrt(static_cast<double>(config("ankle_torque"))); }
    if(config.has("net_wrench")) { netWrenchSqrt = std::sqrt(static_cast<double>(config("net_wrench"))); }
    if(config.has("pressure")) { pressureSqrt = std::sqrt(static_cast<double>(config("pressure"))); }
  }
 
  mc_rtc::Configuration save() const
  {
    mc_rtc::Configuration config;
    config.add("ankle_torque", std::pow(ankleTorqueSqrt, 2));
    config.add("net_wrench", std::pow(netWrenchSqrt, 2));
    config.add("pressure", std::pow(pressureSqrt, 2));
    return config;
  }
};
 
struct MC_RBDYN_DLLAPI FDMPCWeights
{
 
  FDMPCWeights()
  {
    cop_ = 100;
    copRegulation_ = 1e-8;
    copDiff_ = 1e-8;
  }
 
  FDMPCWeights(double cop, double copReg, double copDiff)
  {
    cop_ = cop;
    copRegulation_ = copReg;
    copDiff_ = copDiff;
  }
  double cop_;
  double copRegulation_;
  double copDiff_;
 
  void load(const mc_rtc::Configuration & config)
  {
    if(config.has("cop")) { cop_ = static_cast<double>(config("cop")); }
    if(config.has("cop_regulation")) { copRegulation_ = static_cast<double>(config("cop_regulation")); }
    if(config.has("cop_diff")) { copDiff_ = static_cast<double>(config("cop_diff")); }
  }
 
  mc_rtc::Configuration save() const
  {
    mc_rtc::Configuration config;
    config.add("cop", cop_);
    config.add("cop_regulation", copRegulation_);
    config.add("cop_diff", copDiff_);
    return config;
  }
};
 
struct SafetyThresholds
{
  double MAX_AVERAGE_DCM_ERROR = 0.05; 
  double MAX_COP_ADMITTANCE = 0.1; 
  double MAX_DCM_D_GAIN = 2.; 
  double MAX_DCM_I_GAIN = 100.; 
  double MAX_DCM_P_GAIN = 20.; 
  double MAX_COMD_GAIN = 10.; 
  double MAX_ZMPD_GAIN = 10.; 
  double MAX_DF_ADMITTANCE = 5e-4; 
  double MAX_DF_DAMPING = 10.; 
  double MAX_FDC_RX_VEL = 0.2; 
  double MAX_FDC_RY_VEL = 0.2; 
  double MAX_FDC_RZ_VEL = 0.2; 
  double MIN_DS_PRESSURE = 15.; 
  double MIN_NET_TOTAL_FORCE_ZMP = 1.;
 
  void load(const mc_rtc::Configuration & config)
  {
    config("MAX_AVERAGE_DCM_ERROR", MAX_AVERAGE_DCM_ERROR);
    config("MAX_COP_ADMITTANCE", MAX_COP_ADMITTANCE);
    config("MAX_DCM_D_GAIN", MAX_DCM_D_GAIN);
    config("MAX_DCM_I_GAIN", MAX_DCM_I_GAIN);
    config("MAX_DCM_P_GAIN", MAX_DCM_P_GAIN);
    config("MAX_COMD_GAIN", MAX_COMD_GAIN);
    config("MAX_ZMPD_GAIN", MAX_ZMPD_GAIN);
    config("MAX_DF_ADMITTANCE", MAX_DF_ADMITTANCE);
    if(config.has("MAX_DFZ_ADMITTANCE"))
    {
      mc_rtc::log::deprecated("StabilizerConfiguration::SafetyThresholds", "MAX_DFZ_ADMITTANCE", "MAX_DF_ADMITTANCE");
      config("MAX_DFZ_ADMITTANCE", MAX_DF_ADMITTANCE);
    }
    config("MAX_DF_DAMPING", MAX_DF_ADMITTANCE);
    if(config.has("MAX_DFZ_DAMPING"))
    {
      mc_rtc::log::deprecated("StabilizerConfiguration::SafetyThresholds", "MAX_DFZ_DAMPING", "MAX_DF_ADMITTANCE");
      config("MAX_DFZ_DAMPING", MAX_DF_ADMITTANCE);
    }
    config("MAX_FDC_RX_VEL", MAX_FDC_RX_VEL);
    config("MAX_FDC_RY_VEL", MAX_FDC_RY_VEL);
    config("MAX_FDC_RZ_VEL", MAX_FDC_RZ_VEL);
    config("MIN_DS_PRESSURE", MIN_DS_PRESSURE);
    config("MIN_NET_TOTAL_FORCE_ZMP", MIN_NET_TOTAL_FORCE_ZMP);
  }
 
  mc_rtc::Configuration save() const
  {
    mc_rtc::Configuration config;
    config.add("MAX_AVERAGE_DCM_ERROR", MAX_AVERAGE_DCM_ERROR);
    config.add("MAX_COP_ADMITTANCE", MAX_COP_ADMITTANCE);
    config.add("MAX_DCM_D_GAIN", MAX_DCM_D_GAIN);
    config.add("MAX_DCM_I_GAIN", MAX_DCM_I_GAIN);
    config.add("MAX_DCM_P_GAIN", MAX_DCM_P_GAIN);
    config.add("MAX_COMD_GAIN", MAX_COMD_GAIN);
    config.add("MAX_ZMPD_GAIN", MAX_ZMPD_GAIN);
    config.add("MAX_DF_ADMITTANCE", MAX_DF_ADMITTANCE);
    config.add("MAX_DF_DAMPING", MAX_DF_DAMPING);
    config.add("MAX_FDC_RX_VEL", MAX_FDC_RX_VEL);
    config.add("MAX_FDC_RY_VEL", MAX_FDC_RY_VEL);
    config.add("MAX_FDC_RZ_VEL", MAX_FDC_RZ_VEL);
    config.add("MIN_DS_PRESSURE", MIN_DS_PRESSURE);
    config.add("MIN_NET_TOTAL_FORCE_ZMP", MIN_NET_TOTAL_FORCE_ZMP);
    return config;
  }
};
 
struct DCMBiasEstimatorConfiguration
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  double dcmMeasureErrorStd = 0.01;
  double zmpMeasureErrorStd = 0.05;
  double biasDriftPerSecondStd = 0.02;
  Eigen::Vector2d biasLimit = {0.02, 0.02};
  Eigen::Vector2d comBiasLimit = biasLimit;
  bool withDCMBias = false;
  bool withDCMFilter = false;
  bool correctCoMPos = false;
  bool correctDCM = true;
 
  void load(const mc_rtc::Configuration & config)
  {
    config("dcmMeasureErrorStd", dcmMeasureErrorStd);
    config("zmpMeasureErrorStd", zmpMeasureErrorStd);
    config("biasDriftPerSecondStd", biasDriftPerSecondStd);
    config("biasLimit", biasLimit);
    config("comBiasLimit", comBiasLimit);
    config("withDCMBias", withDCMBias);
    config("correctCoMPos", correctCoMPos);
    config("withDCMFilter", withDCMFilter);
    config("correctDCM", correctDCM);
  }
 
  mc_rtc::Configuration save() const
  {
    mc_rtc::Configuration config;
    config.add("dcmMeasureErrorStd", dcmMeasureErrorStd);
    config.add("zmpMeasureErrorStd", zmpMeasureErrorStd);
    config.add("biasDriftPerSecondStd", biasDriftPerSecondStd);
    config.add("biasLimit", biasLimit);
    config.add("comBiasLimit", comBiasLimit);
    config.add("withDCMBias", withDCMBias);
    config.add("correctCoMPos", correctCoMPos);
    config.add("correctDCM", correctDCM);
    config.add("withDCMFilter", withDCMFilter);
    return config;
  }
};
 
struct ExternalWrenchConfiguration
{
  bool addExpectedCoMOffset = false;
  bool subtractMeasuredValue = false;
  bool modifyCoMErr = false;
  bool modifyZMPErr = false;
  bool modifyZMPErrD = false;
  bool excludeFromDCMBiasEst = false;
  double comOffsetErrCoMLimit = 0.1;
  double comOffsetErrZMPLimit = 0.1;
  double extWrenchSumLowPassCutoffPeriod = 0.05;
  double comOffsetLowPassCutoffPeriod = 0.05;
  double comOffsetLowPassCoMCutoffPeriod = 1.0;
  double comOffsetDerivatorTimeConstant = 1.0;
 
  void load(const mc_rtc::Configuration & config)
  {
    config("add_expected_com_offset", addExpectedCoMOffset);
    config("subtract_measured_value", subtractMeasuredValue);
    config("modify_com_error", modifyCoMErr);
    config("modify_zmp_error", modifyZMPErr);
    config("modify_zmp_error_d", modifyZMPErrD);
    config("exclude_from_dcm_bias_est", excludeFromDCMBiasEst);
    config("com_offset_err_com_limit", comOffsetErrCoMLimit);
    config("com_offset_err_zmp_limit", comOffsetErrZMPLimit);
    config("ext_wrench_sum_cutoff", extWrenchSumLowPassCutoffPeriod);
    config("com_offset_cutoff", comOffsetLowPassCutoffPeriod);
    config("com_offset_com_cutoff", comOffsetLowPassCoMCutoffPeriod);
    config("derivator_time_constant", comOffsetDerivatorTimeConstant);
  }
 
  mc_rtc::Configuration save() const
  {
    mc_rtc::Configuration config;
    config.add("add_expected_com_offset", addExpectedCoMOffset);
    config.add("subtract_measured_value", subtractMeasuredValue);
    config.add("modify_com_error", modifyCoMErr);
    config.add("modify_zmp_error", modifyZMPErr);
    config.add("modify_zmp_error_d", modifyZMPErrD);
    config.add("exclude_from_dcm_bias_est", excludeFromDCMBiasEst);
    config.add("com_offset_err_com_limit", comOffsetErrCoMLimit);
    config.add("com_offset_err_zmp_limit", comOffsetErrZMPLimit);
    config.add("ext_wrench_sum_cutoff", extWrenchSumLowPassCutoffPeriod);
    config.add("com_offset_cutoff", comOffsetLowPassCutoffPeriod);
    config.add("com_offset_com_cutoff", comOffsetLowPassCoMCutoffPeriod);
    config.add("derivator_time_constant", comOffsetDerivatorTimeConstant);
    return config;
  }
};
 
} // namespace lipm_stabilizer
} // namespace mc_rbdyn
 
namespace mc_rtc
{
template<>
struct ConfigurationLoader<mc_rbdyn::lipm_stabilizer::FDQPWeights>
{
  static mc_rbdyn::lipm_stabilizer::FDQPWeights load(const mc_rtc::Configuration & config)
  {
    mc_rbdyn::lipm_stabilizer::FDQPWeights weights;
    weights.load(config);
    return weights;
  }
 
  static mc_rtc::Configuration save(const mc_rbdyn::lipm_stabilizer::FDQPWeights & weights) { return weights.save(); }
};
 
template<>
struct ConfigurationLoader<mc_rbdyn::lipm_stabilizer::FDMPCWeights>
{
  static mc_rbdyn::lipm_stabilizer::FDMPCWeights load(const mc_rtc::Configuration & config)
  {
    mc_rbdyn::lipm_stabilizer::FDMPCWeights weights;
    weights.load(config);
    return weights;
  }
 
  static mc_rtc::Configuration save(const mc_rbdyn::lipm_stabilizer::FDMPCWeights & weights) { return weights.save(); }
};
 
template<>
struct ConfigurationLoader<mc_rbdyn::lipm_stabilizer::SafetyThresholds>
{
  static mc_rbdyn::lipm_stabilizer::SafetyThresholds load(const mc_rtc::Configuration & config)
  {
    mc_rbdyn::lipm_stabilizer::SafetyThresholds safety;
    safety.load(config);
    return safety;
  }
 
  static mc_rtc::Configuration save(const mc_rbdyn::lipm_stabilizer::SafetyThresholds & safety)
  {
    return safety.save();
  }
};
 
template<>
struct ConfigurationLoader<mc_rbdyn::lipm_stabilizer::DCMBiasEstimatorConfiguration>
{
  static mc_rbdyn::lipm_stabilizer::DCMBiasEstimatorConfiguration load(const mc_rtc::Configuration & config)
  {
    mc_rbdyn::lipm_stabilizer::DCMBiasEstimatorConfiguration bias;
    bias.load(config);
    return bias;
  }
 
  static mc_rtc::Configuration save(const mc_rbdyn::lipm_stabilizer::DCMBiasEstimatorConfiguration & bias)
  {
    return bias.save();
  }
};
 
template<>
struct ConfigurationLoader<mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration>
{
  static mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration load(const mc_rtc::Configuration & config)
  {
    mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration extWrench;
    extWrench.load(config);
    return extWrench;
  }
 
  static mc_rtc::Configuration save(const mc_rbdyn::lipm_stabilizer::ExternalWrenchConfiguration & extWrench)
  {
    return extWrench.save();
  }
};
} // namespace mc_rtc
 
namespace mc_rbdyn
{
namespace lipm_stabilizer
{
 
struct MC_RBDYN_DLLAPI StabilizerConfiguration
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  bool verbose = true; 
  SafetyThresholds safetyThresholds;
  FDQPWeights fdqpWeights;
  FDMPCWeights fdmpcWeights;
 
  double friction = 0.7; 
  std::string leftFootSurface; 
  std::string rightFootSurface; 
  bool constrainCoP = true; 
  bool useTargetPressure = true; 
  Eigen::Vector2d copAdmittance = Eigen::Vector2d::Zero(); 
  sva::MotionVecd copMaxVel{{0.3, 0.3, 0.3}, {0.1, 0.1, 0.1}}; 
  double copVelFilterGain = 0.8; 
  ZMPCCConfiguration zmpcc; 
  mc_rbdyn::Gains3d dfAdmittance = {0, 0, 1e-4}; 
  mc_rbdyn::Gains3d dfDamping = 0.; 
  mc_rbdyn::Gains2d dcmPropGain = 1.; 
  mc_rbdyn::Gains2d dcmIntegralGain = 5.; 
  mc_rbdyn::Gains2d dcmDerivGain = 0.; 
  double comdErrorGain = 1.; 
  double zmpdGain = 0.; 
  double dcmIntegratorTimeConstant =
      15.; 
  double dcmDerivatorTimeConstant = 1.; 
  double fSumFilterTimeConstant = 20.; 
  std::vector<std::string> comActiveJoints; 
  Eigen::Vector3d comStiffness = {1000., 1000., 100.}; 
  double comWeight = 1000.; 
  Eigen::Vector3d comDimWeight = Eigen::Vector3d::Ones(); 
  double comHeight = 0.84; 
  Eigen::Vector3d copFzLambda = 100.0 * Eigen::Vector3d::Ones();
  double delayCoP = 0;
 
  std::string torsoBodyName; 
  double torsoPitch = 0; 
  double torsoStiffness = 10; 
  double torsoWeight = 100; 
  Eigen::Vector3d torsoDimWeight = Eigen::Vector3d::Ones(); 
  double pelvisStiffness = 10; 
  double pelvisWeight = 100; 
  Eigen::Vector3d pelvisDimWeight = Eigen::Vector3d::Ones(); 
  sva::MotionVecd contactDamping{{300, 300, 300},
                                 {300, 300, 300}}; 
  sva::MotionVecd contactStiffness = {{1, 1, 1}, {1, 1, 1}}; 
  double contactWeight = 100000.; 
  double vdcFrequency = 1.; 
  double vdcStiffness = 1000.; 
  DCMBiasEstimatorConfiguration dcmBias; 
  ExternalWrenchConfiguration extWrench; 
  StabilizerConfiguration() {}
 
  StabilizerConfiguration(const mc_rtc::Configuration & conf) { load(conf); }
 
  void clampGains()
  {
    using ::mc_filter::utils::clampInPlaceAndWarn;
    const auto & s = safetyThresholds;
    clampInPlaceAndWarn(copAdmittance.x(), 0., s.MAX_COP_ADMITTANCE, "CoP x-admittance");
    clampInPlaceAndWarn(copAdmittance.y(), 0., s.MAX_COP_ADMITTANCE, "CoP y-admittance");
    clampInPlaceAndWarn(dcmDerivGain, 0., s.MAX_DCM_D_GAIN, "DCM deriv gain");
    clampInPlaceAndWarn(dcmIntegralGain, 0., s.MAX_DCM_I_GAIN, "DCM integral gain");
    clampInPlaceAndWarn(dcmPropGain, 0., s.MAX_DCM_P_GAIN, "DCM prop gain");
    clampInPlaceAndWarn(comdErrorGain, 0., s.MAX_COMD_GAIN, "CoMd gain");
    clampInPlaceAndWarn(zmpdGain, 0., s.MAX_ZMPD_GAIN, "ZMPd gain");
    clampInPlaceAndWarn(dfAdmittance, 0., s.MAX_DF_ADMITTANCE, "DF admittance");
    clampInPlaceAndWarn(dfDamping, 0., s.MAX_DF_DAMPING, "DF admittance");
  }
 
  void load(const mc_rtc::Configuration & config)
  {
    config("verbose", verbose);
 
    if(config.has("safety_tresholds")) { safetyThresholds.load(config("safety_tresholds")); }
 
    if(config.has("fdqp_weights")) { fdqpWeights.load(config("fdqp_weights")); }
 
    if(config.has("fdmpc_weights")) { fdmpcWeights.load(config("fdmpc_weights")); }
 
    config("friction", friction);
    config("leftFootSurface", leftFootSurface);
    config("rightFootSurface", rightFootSurface);
    config("torsoBodyName", torsoBodyName);
 
    if(config.has("admittance"))
    {
      auto admittance = config("admittance");
      admittance("cop", copAdmittance);
      admittance("useTargetPressure", useTargetPressure);
      admittance("copFzLambda", copFzLambda);
      admittance("copFzDelay", delayCoP);
      admittance("maxVel", copMaxVel);
      admittance("velFilterGain", mc_filter::utils::clamp(copVelFilterGain, 0, 1));
      if(admittance.has("dfz"))
      {
        mc_rtc::log::warning("[MC_RTC_DEPRECATED][StabilizerConfiguration] dfz is now dimensional, to "
                             "keep the same behavior use df: [0, 0, dfz]");
        dfAdmittance.setZero();
        dfAdmittance.z() = admittance("dfz");
      }
      else { admittance("df", dfAdmittance); }
      if(admittance.has("dfz_damping"))
      {
        mc_rtc::log::warning("[MC_RTC_DEPRECATED][StabilizerConfiguration] dfz_damping is now dimensional, to "
                             "keep the same behavior use df_damping: [0, 0, dfz_damping]");
        dfDamping.setZero();
        dfDamping.z() = admittance("dfz_damping");
      }
      else { admittance("df_damping", dfDamping); }
    }
    if(config.has("dcm_tracking"))
    {
      auto dcmTracking = config("dcm_tracking");
      if(dcmTracking.has("gains"))
      {
        dcmTracking("gains")("prop", dcmPropGain);
        dcmTracking("gains")("integral", dcmIntegralGain);
        dcmTracking("gains")("deriv", dcmDerivGain);
        dcmTracking("gains")("comdError", comdErrorGain);
        dcmTracking("gains")("zmpd", zmpdGain);
      }
      if(dcmTracking.has("derivator_time_constant"))
      {
        mc_rtc::log::warning("derivator_time_constant is deprecated, use derivator_cutoff_period instead");
        dcmTracking("derivator_time_constant", dcmDerivatorTimeConstant);
      }
      dcmTracking("derivator_cutoff_period", dcmDerivatorTimeConstant);
      dcmTracking("integrator_time_constant", dcmIntegratorTimeConstant);
    }
    if(config.has("dcm_bias")) { dcmBias.load(config("dcm_bias")); }
    if(config.has("external_wrench")) { extWrench.load(config("external_wrench")); }
    if(config.has("tasks"))
    {
      auto tasks = config("tasks");
      if(tasks.has("com"))
      {
        tasks("com")("active_joints", comActiveJoints);
        tasks("com")("stiffness", comStiffness);
        tasks("com")("weight", comWeight);
        tasks("com")("dimWeight", comDimWeight);
        tasks("com")("height", comHeight);
      }
 
      if(tasks.has("torso"))
      {
 
        tasks("torso")("pitch", torsoPitch);
        tasks("torso")("stiffness", torsoStiffness);
        tasks("torso")("weight", torsoWeight);
        tasks("torso")("dimWeight", torsoDimWeight);
      }
 
      if(tasks.has("pelvis"))
      {
        tasks("pelvis")("stiffness", pelvisStiffness);
        tasks("pelvis")("weight", pelvisWeight);
        tasks("pelvis")("dimWeight", pelvisDimWeight);
      }
 
      if(tasks.has("contact"))
      {
        tasks("contact")("constrainCoP", constrainCoP);
        if(tasks("contact").has("damping"))
        {
          try
          {
            double d = tasks("contact")("damping");
            contactDamping = sva::MotionVecd({d, d, d}, {d, d, d});
          }
          catch(mc_rtc::Configuration::Exception & e)
          {
            e.silence();
            contactDamping = tasks("contact")("damping");
          }
        }
        if(tasks("contact").has("stiffness"))
        {
          try
          {
            double k = tasks("contact")("stiffness");
            contactStiffness = sva::MotionVecd({k, k, k}, {k, k, k});
          }
          catch(mc_rtc::Configuration::Exception & e)
          {
            e.silence();
            contactStiffness = tasks("contact")("stiffness");
          }
        }
        tasks("contact")("stiffness", contactStiffness);
        tasks("contact")("weight", contactWeight);
      }
    }
    if(config.has("vdc"))
    {
      auto vdc = config("vdc");
      vdc("frequency", vdcFrequency);
      vdc("stiffness", vdcStiffness);
    }
 
    if(config.has("zmpcc")) { zmpcc.load(config("zmpcc")); }
    config("zmpcc", zmpcc);
  }
 
  mc_rtc::Configuration save() const
  {
    mc_rtc::Configuration conf;
    conf.add("verbose", verbose);
 
    conf.add("safety_tresholds", safetyThresholds);
    conf.add("fdqp_weights", fdqpWeights);
    conf.add("fdmpc_weights", fdmpcWeights);
 
    conf.add("friction", friction);
    conf.add("leftFootSurface", leftFootSurface);
    conf.add("rightFootSurface", rightFootSurface);
    conf.add("torsoBodyName", torsoBodyName);
 
    conf.add("admittance");
    conf("admittance").add("cop", copAdmittance);
    conf("admittance").add("useTargetPressure", useTargetPressure);
    conf("admittance").add("df", dfAdmittance);
    conf("admittance").add("df_damping", dfDamping);
    conf("admittance").add("maxVel", copMaxVel);
    conf("admittance").add("velFilterGain", copVelFilterGain);
 
    conf.add("zmpcc", zmpcc);
 
    conf.add("dcm_tracking");
    conf("dcm_tracking").add("gains");
    conf("dcm_tracking")("gains").add("prop", dcmPropGain);
    conf("dcm_tracking")("gains").add("integral", dcmIntegralGain);
    conf("dcm_tracking")("gains").add("deriv", dcmDerivGain);
    conf("dcm_tracking")("gains").add("comdError", comdErrorGain);
    conf("dcm_tracking")("gains").add("zmpd", zmpdGain);
    conf("dcm_tracking").add("derivator_cutoff_period", dcmDerivatorTimeConstant);
    conf("dcm_tracking").add("integrator_time_constant", dcmIntegratorTimeConstant);
 
    conf.add("dcm_bias", dcmBias);
    conf.add("external_wrench", extWrench);
 
    conf.add("tasks");
    conf("tasks").add("com");
    conf("tasks")("com").add("active_joints", comActiveJoints);
    conf("tasks")("com").add("stiffness", comStiffness);
    conf("tasks")("com").add("weight", comWeight);
    conf("tasks")("com").add("dimWeight", comDimWeight);
    conf("tasks")("com").add("height", comHeight);
 
    conf("tasks").add("torso");
    conf("tasks")("torso").add("pitch", torsoPitch);
    conf("tasks")("torso").add("stiffness", torsoStiffness);
    conf("tasks")("torso").add("weight", torsoWeight);
    conf("tasks")("torso").add("dimWeight", torsoDimWeight);
 
    conf("tasks").add("pelvis");
    conf("tasks")("pelvis").add("stiffness", pelvisStiffness);
    conf("tasks")("pelvis").add("weight", pelvisWeight);
    conf("tasks")("pelvis").add("dimWeight", pelvisDimWeight);
 
    conf("tasks").add("contact");
    conf("tasks")("contact").add("damping", contactDamping);
    conf("tasks")("contact").add("stiffness", contactStiffness);
    conf("tasks")("contact").add("weight", contactWeight);
 
    conf.add("vdc");
    conf("vdc").add("frequency", vdcFrequency);
    conf("vdc").add("stiffness", vdcStiffness);
    return conf;
  }
};
} // namespace lipm_stabilizer
} // namespace mc_rbdyn
 
namespace mc_rtc
{
template<>
struct ConfigurationLoader<mc_rbdyn::lipm_stabilizer::StabilizerConfiguration>
{
  static mc_rbdyn::lipm_stabilizer::StabilizerConfiguration load(const mc_rtc::Configuration & config)
  {
    mc_rbdyn::lipm_stabilizer::StabilizerConfiguration stabi;
    stabi.load(config);
    return stabi;
  }
 
  static mc_rtc::Configuration save(const mc_rbdyn::lipm_stabilizer::StabilizerConfiguration & stabiConf)
  {
    return stabiConf.save();
  }
};
} // namespace mc_rtc