Stabilizer.h

Go to the documentation of this file.

/*
 * Copyright (c) 2018-2019, CNRS-UM LIRMM
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#pragma once

#include <mc_tasks/CoMTask.h>
#include <mc_tasks/CoPTask.h>

#include <eigen-quadprog/QuadProg.h>
#include <lipm_walking/Contact.h>
#include <lipm_walking/Pendulum.h>
#include <lipm_walking/Sole.h>
#include <lipm_walking/utils/ExponentialMovingAverage.h>
#include <lipm_walking/utils/LeakyIntegrator.h>
#include <lipm_walking/utils/StationaryOffsetFilter.h>

namespace lipm_walking
{

struct Stabilizer
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  static constexpr double MAX_AVERAGE_DCM_ERROR = 0.05; 
  static constexpr double MAX_COM_ADMITTANCE = 20; 
  static constexpr double MAX_COP_ADMITTANCE = 0.1; 
  static constexpr double MAX_DCM_D_GAIN = 2.; 
  static constexpr double MAX_DCM_I_GAIN = 100.; 
  static constexpr double MAX_DCM_P_GAIN = 20.; 
  static constexpr double MAX_DFZ_ADMITTANCE =
      5e-4; 
  static constexpr double MAX_DFZ_DAMPING =
      10.; 
  static constexpr double MAX_FDC_RX_VEL =
      0.2; 
  static constexpr double MAX_FDC_RY_VEL =
      0.2; 
  static constexpr double MAX_FDC_RZ_VEL =
      0.2; 
  static constexpr double MAX_ZMPCC_COM_OFFSET = 0.05; 
  static constexpr double MIN_DSP_FZ = 15.; 
  Stabilizer(const mc_rbdyn::Robot & robot, const Pendulum & pendulum, double dt);

  void addGUIElements(std::shared_ptr<mc_rtc::gui::StateBuilder> gui);

  void addLogEntries(mc_rtc::Logger & logger);

  void addTasks(mc_solver::QPSolver & solver);

  void disable();

  Eigen::Vector3d computeZMP(const sva::ForceVecd & wrench) const;

  void configure(const mc_rtc::Configuration &);

  bool detectTouchdown(const std::shared_ptr<mc_tasks::force::CoPTask> footTask, const Contact & contact);

  void reconfigure();

  void removeTasks(mc_solver::QPSolver & solver);

  void reset(const mc_rbdyn::Robots & robots);

  void run();

  void seekTouchdown(std::shared_ptr<mc_tasks::force::CoPTask> footTask);

  void setContact(std::shared_ptr<mc_tasks::force::CoPTask> footTask, const Contact & contact);

  void setSwingFoot(std::shared_ptr<mc_tasks::force::CoPTask> footTask);

  ContactState contactState()
  {
    return contactState_;
  }

  void contactState(ContactState contactState)
  {
    contactState_ = contactState;
  }

  void sole(const Sole & sole)
  {
    sole_ = sole;
  }

  void updateState(const Eigen::Vector3d & com,
                   const Eigen::Vector3d & comd,
                   const sva::ForceVecd & wrench,
                   double leftFootRatio);

  void wrenchFaceMatrix(const Sole & sole)
  {
    double X = sole.halfLength;
    double Y = sole.halfWidth;
    double mu = sole.friction;
    // clang-format off
    wrenchFaceMatrix_ <<
      // mx,  my,  mz,  fx,  fy,            fz,
          0,   0,   0,  -1,   0,           -mu,
          0,   0,   0,  +1,   0,           -mu,
          0,   0,   0,   0,  -1,           -mu,
          0,   0,   0,   0,  +1,           -mu,
         -1,   0,   0,   0,   0,            -Y,
         +1,   0,   0,   0,   0,            -Y,
          0,  -1,   0,   0,   0,            -X,
          0,  +1,   0,   0,   0,            -X,
        +mu, +mu,  -1,  -Y,  -X, -(X + Y) * mu,
        +mu, -mu,  -1,  -Y,  +X, -(X + Y) * mu,
        -mu, +mu,  -1,  +Y,  -X, -(X + Y) * mu,
        -mu, -mu,  -1,  +Y,  +X, -(X + Y) * mu,
        +mu, +mu,  +1,  +Y,  +X, -(X + Y) * mu,
        +mu, -mu,  +1,  +Y,  -X, -(X + Y) * mu,
        -mu, +mu,  +1,  -Y,  +X, -(X + Y) * mu,
        -mu, -mu,  +1,  -Y,  -X, -(X + Y) * mu;
    // clang-format on
  }

  Eigen::Vector3d zmp() const
  {
    return computeZMP(distribWrench_);
  }

private:
  struct FDQPWeights
  {
    void configure(const mc_rtc::Configuration & config)
    {
      double ankleTorqueWeight = config("ankle_torque");
      double forceRatioWeight = config("force_ratio");
      double netWrenchWeight = config("net_wrench");
      ankleTorqueSqrt = std::sqrt(ankleTorqueWeight);
      forceRatioSqrt = std::sqrt(forceRatioWeight);
      netWrenchSqrt = std::sqrt(netWrenchWeight);
    }

  public:
    double ankleTorqueSqrt;
    double forceRatioSqrt;
    double netWrenchSqrt;
  };

  void checkGains();

  void checkInTheAir();

  sva::ForceVecd computeDesiredWrench();

  void distributeWrench(const sva::ForceVecd & desiredWrench);
  void distributeWrenchQuadProg(const sva::ForceVecd & desiredWrench);

  void saturateWrench(const sva::ForceVecd & desiredWrench, std::shared_ptr<mc_tasks::force::CoPTask> & footTask);
  void saturateWrenchQuadProg(const sva::ForceVecd & desiredWrench,
                              std::shared_ptr<mc_tasks::force::CoPTask> & footTask);

  void setSupportFootGains();

  void updateCoMTaskZMPCC();

  void updateFootForceDifferenceControl();

  void updateZMPFrame();

  sva::ForceVecd contactAdmittance()
  {
    return {{copAdmittance_.y(), copAdmittance_.x(), 0.}, {0., 0., 0.}};
  }

public:
  Contact leftFootContact; 
  Contact rightFootContact; 
  std::shared_ptr<mc_tasks::CoMTask> comTask; 
  std::shared_ptr<mc_tasks::force::CoPTask> leftFootTask; 
  std::shared_ptr<mc_tasks::force::CoPTask> rightFootTask; 
private:
  ContactState contactState_ = ContactState::DoubleSupport; 
  Eigen::Matrix<double, 16, 6> wrenchFaceMatrix_; 
  Eigen::QuadProgDense qpSolver_; 
  Eigen::Vector2d comAdmittance_ = Eigen::Vector2d::Zero(); 
  Eigen::Vector2d copAdmittance_ = Eigen::Vector2d::Zero(); 
  Eigen::Vector3d comStiffness_ = {1000., 1000., 100.}; 
  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 measuredZMP_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d zmpError_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d zmpccCoMAccel_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d zmpccCoMOffset_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d zmpccCoMVel_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector3d zmpccError_ = Eigen::Vector3d::Zero(); 
  Eigen::Vector4d polePlacement_ = {-10., -5., -1., 10.}; 
  ExponentialMovingAverage dcmIntegrator_; 
  FDQPWeights fdqpWeights_; 
  LeakyIntegrator zmpccIntegrator_; 
  StationaryOffsetFilter dcmDerivator_; 
  bool inTheAir_ = false; 
  bool zmpccOnlyDS_ = true; 
  const Pendulum & pendulum_; 
  const mc_rbdyn::Robot & controlRobot_; 
  double comWeight_ = 1000.; 
  double contactWeight_ = 100000.; 
  double dcmDerivGain_ = 0.; 
  double dcmIntegralGain_ = 5.; 
  double dcmPropGain_ = 1.; 
  double dfzAdmittance_ = 1e-4; 
  double dfzDamping_ = 0.; 
  double dfzForceError_ = 0.; 
  double dfzHeightError_ = 0.; 
  double dt_ = 0.005; 
  double leftFootRatio_ = 0.5; 
  double mass_ = 38.; 
  double runTime_ = 0.; 
  double swingFootStiffness_ = 2000.; 
  double swingFootWeight_ = 500.; 
  double vdcFrequency_ = 1.; 
  double vdcHeightError_ = 0.; 
  double vdcStiffness_ = 1000.; 
  Sole sole_; 
  mc_rtc::Configuration config_; 
  std::vector<std::string> comActiveJoints_; 
  sva::ForceVecd distribWrench_ = sva::ForceVecd::Zero(); 
  sva::ForceVecd measuredWrench_ = sva::ForceVecd::Zero(); 
  sva::MotionVecd contactDamping_ = sva::MotionVecd::Zero(); 
  sva::MotionVecd contactStiffness_ = sva::MotionVecd::Zero(); 
  sva::PTransformd zmpFrame_ = sva::PTransformd::Identity(); 
};

} // namespace lipm_walking