ZMPCC.h

Go to the documentation of this file.

/*
 * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
#include <mc_filter/LeakyIntegrator.h>
#include <mc_rbdyn/lipm_stabilizer/ZMPCCConfiguration.h>
#include <SpaceVecAlg/SpaceVecAlg>
 
namespace mc_rtc
{
struct Logger;
namespace gui
{
struct StateBuilder;
} // namespace gui
} // namespace mc_rtc
 
namespace mc_tasks
{
namespace lipm_stabilizer
{
struct ZMPCC
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using ZMPCCConfiguration = mc_rbdyn::lipm_stabilizer::ZMPCCConfiguration;
 
  void reset();
 
  void enabled(bool state) { enabled_ = state; }
 
  bool enabled() const { return enabled_; }
 
  void configure(const ZMPCCConfiguration & config)
  {
    config_ = config;
    integrator_.saturation(config.maxCoMOffset);
    integrator_.rate(config_.integratorLeakRate);
    mc_filter::utils::clampInPlaceAndWarn(config_.comAdmittance.x(), 0., config_.maxCoMAdmitance, "CoM x-admittance");
    mc_filter::utils::clampInPlaceAndWarn(config_.comAdmittance.y(), 0., config_.maxCoMAdmitance, "CoM y-admittance");
  }
 
  const ZMPCCConfiguration & config() const { return config_; }
 
  void update(const Eigen::Vector3d & distribZMP,
              const Eigen::Vector3d & measuredZMP,
              const sva::PTransformd & zmpFrame,
              double dt);
 
  void apply(Eigen::Vector3d & com, Eigen::Vector3d & comd, Eigen::Vector3d & comdd);
 
  void addToGUI(mc_rtc::gui::StateBuilder & gui, const std::vector<std::string> & category);
  void removeFromGUI(mc_rtc::gui::StateBuilder & gui, const std::vector<std::string> & category);
  void addToLogger(mc_rtc::Logger & logger, const std::string & name);
  void removeFromLogger(mc_rtc::Logger & logger, const std::string & name);
 
protected:
  ZMPCCConfiguration config_; 
  Eigen::Vector3d comAccel_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d comOffset_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d comVel_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d error_ = Eigen::Vector3d::Zero(); 
  mc_filter::LeakyIntegrator<Eigen::Vector3d> integrator_; 
  bool enabled_ = true;
};
} // namespace lipm_stabilizer
} // namespace mc_tasks