WrenchDistribution.h

Go to the documentation of this file.

#pragma once
 
#include <qp_solver_collection/QpSolverCollection.h>
 
#include <ForceColl/Constants.h>
#include <ForceColl/Contact.h>
 
namespace ForceColl
{
class WrenchDistribution
{
public:
  struct Configuration
  {
    sva::MotionVecd wrenchWeight = sva::MotionVecd(Eigen::Vector3d(1.0, 1.0, 1.0), Eigen::Vector3d(1.0, 1.0, 1.0));
 
    double regularWeight = 1e-8;
 
    std::pair<double, double> ridgeForceMinMax = std::make_pair(3, 1000); // [N]
 
    void load(const mc_rtc::Configuration & mcRtcConfig);
  };
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  WrenchDistribution(const std::vector<std::shared_ptr<Contact>> & contactList,
                     const mc_rtc::Configuration & mcRtcConfig = {});
 
  sva::ForceVecd run(const sva::ForceVecd & desiredTotalWrench,
                     const Eigen::Vector3d & momentOrigin = Eigen::Vector3d::Zero());
 
  inline const Configuration & config() const noexcept
  {
    return config_;
  }
 
  void addToGUI(mc_rtc::gui::StateBuilder & gui,
                const std::vector<std::string> & category,
                double forceScale = constants::defaultForceScale,
                double fricPyramidScale = constants::defaultFricPyramidScale);
 
public:
  std::vector<std::shared_ptr<Contact>> contactList_;
 
  Eigen::VectorXd resultWrenchRatio_;
 
  sva::ForceVecd desiredTotalWrench_ = sva::ForceVecd::Zero();
 
  sva::ForceVecd resultTotalWrench_ = sva::ForceVecd::Zero();
 
  std::shared_ptr<QpSolverCollection::QpSolver> qpSolver_;
 
  QpSolverCollection::QpCoeff qpCoeff_;
 
protected:
  Configuration config_;
};
} // namespace ForceColl