Contact.h

Go to the documentation of this file.

#pragma once
 
#include <unordered_map>
 
#include <mc_rtc/Configuration.h>
#include <mc_rtc/gui/StateBuilder.h>
#include <SpaceVecAlg/SpaceVecAlg>
 
#include <ForceColl/Constants.h>
 
namespace ForceColl
{
class FrictionPyramid
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  FrictionPyramid(double fricCoeff, int ridgeNum = 4);
 
  std::vector<Eigen::Vector3d> calcGlobalRidgeList(const Eigen::Matrix3d & rot) const;
 
  inline int ridgeNum() const
  {
    return static_cast<int>(localRidgeList_.size());
  }
 
public:
  std::vector<Eigen::Vector3d> localRidgeList_;
};
 
class Contact
{
public:
  struct VertexWithRidge
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    Eigen::Vector3d vertex;
 
    std::vector<Eigen::Vector3d> ridgeList;
 
    VertexWithRidge(const Eigen::Vector3d & _vertex, const std::vector<Eigen::Vector3d> & _ridgeList)
    : vertex(_vertex), ridgeList(_ridgeList)
    {
    }
  };
 
public:
  static std::shared_ptr<Contact> makeSharedFromConfig(const mc_rtc::Configuration & mcRtcConfig);
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  Contact(const std::string & name, std::optional<sva::ForceVecd> maxWrench = std::nullopt);
 
  virtual std::string type() const = 0;
 
  inline int ridgeNum() const
  {
    return static_cast<int>(graspMat_.cols());
  }
 
  virtual void updateGlobalVertices(const sva::PTransformd & pose) = 0;
 
  sva::ForceVecd calcWrench(const Eigen::VectorXd & wrenchRatio,
                            const Eigen::Vector3d & momentOrigin = Eigen::Vector3d::Zero()) const;
 
  sva::ForceVecd calcLocalWrench(const Eigen::VectorXd & wrenchRatio) const;
 
  virtual void addToGUI(mc_rtc::gui::StateBuilder & gui,
                        const std::vector<std::string> & category,
                        double forceScale = constants::defaultForceScale,
                        double fricPyramidScale = constants::defaultFricPyramidScale,
                        const Eigen::VectorXd & wrenchRatio = Eigen::VectorXd::Zero(0));
 
public:
  std::string name_;
 
  Eigen::Matrix<double, 6, Eigen::Dynamic> graspMat_;
 
  Eigen::Matrix<double, 6, Eigen::Dynamic> localGraspMat_;
 
  std::shared_ptr<FrictionPyramid> fricPyramid_;
 
  std::vector<VertexWithRidge> vertexWithRidgeList_;
 
  std::optional<sva::ForceVecd> maxWrench_;
};
 
class EmptyContact : public Contact
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  EmptyContact(const std::string & name);
 
  EmptyContact(const mc_rtc::Configuration & mcRtcConfig);
 
  inline virtual std::string type() const override
  {
    return "Empty";
  }
 
  virtual void updateGlobalVertices(const sva::PTransformd & pose) override;
};
 
class SurfaceContact : public Contact
{
public:
  static void loadVerticesMap(const mc_rtc::Configuration & mcRtcConfig);
 
  static inline std::unordered_map<std::string, std::vector<Eigen::Vector3d>> verticesMap;
 
  std::vector<Eigen::Vector3d> localVertices_;
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  SurfaceContact(const std::string & name,
                 double fricCoeff,
                 const std::vector<Eigen::Vector3d> & localVertices,
                 const sva::PTransformd & pose,
                 std::optional<sva::ForceVecd> maxWrench = std::nullopt);
 
  SurfaceContact(const mc_rtc::Configuration & mcRtcConfig);
 
  inline virtual std::string type() const override
  {
    return "Surface";
  }
 
  void updateLocalVertices(const std::vector<Eigen::Vector3d> & localVertices);
 
  virtual void updateGlobalVertices(const sva::PTransformd & pose) override;
 
  virtual void addToGUI(mc_rtc::gui::StateBuilder & gui,
                        const std::vector<std::string> & category,
                        double forceScale = constants::defaultForceScale,
                        double fricPyramidScale = constants::defaultFricPyramidScale,
                        const Eigen::VectorXd & wrenchRatio = Eigen::VectorXd::Zero(0)) override;
};
 
class GraspContact : public Contact
{
public:
  static void loadVerticesMap(const mc_rtc::Configuration & mcRtcConfig);
 
  static inline std::unordered_map<std::string, std::vector<sva::PTransformd>> verticesMap;
 
  std::vector<sva::PTransformd> localVertices_;
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  GraspContact(const std::string & name,
               double fricCoeff,
               const std::vector<sva::PTransformd> & localVertices,
               const sva::PTransformd & pose,
               std::optional<sva::ForceVecd> maxWrench = std::nullopt);
 
  GraspContact(const mc_rtc::Configuration & mcRtcConfig);
 
  inline virtual std::string type() const override
  {
    return "Grasp";
  }
 
  void updateLocalVertices(const std::vector<sva::PTransformd> & localVertices);
 
  virtual void updateGlobalVertices(const sva::PTransformd & pose) override;
 
  virtual void addToGUI(mc_rtc::gui::StateBuilder & gui,
                        const std::vector<std::string> & category,
                        double forceScale = constants::defaultForceScale,
                        double fricPyramidScale = constants::defaultFricPyramidScale,
                        const Eigen::VectorXd & wrenchRatio = Eigen::VectorXd::Zero(0)) override;
};
 
sva::ForceVecd calcTotalWrench(const std::vector<std::shared_ptr<Contact>> & contactList,
                               const Eigen::VectorXd & wrenchRatio,
                               const Eigen::Vector3d & momentOrigin = Eigen::Vector3d::Zero());
 
std::vector<sva::ForceVecd> calcWrenchList(const std::vector<std::shared_ptr<Contact>> & contactList,
                                           const Eigen::VectorXd & wrenchRatio,
                                           const Eigen::Vector3d & momentOrigin = Eigen::Vector3d::Zero());
 
std::vector<sva::ForceVecd> calcLocalWrenchList(const std::vector<std::shared_ptr<Contact>> & contactList,
                                                const Eigen::VectorXd & wrenchRatio);
 
template<template<class...> class MapType, class KeyType, class... RestTypes>
MapType<KeyType, sva::ForceVecd> calcWrenchList(
    const MapType<KeyType, std::shared_ptr<Contact>, RestTypes...> & contactList,
    const Eigen::VectorXd & wrenchRatio,
    const Eigen::Vector3d & momentOrigin = Eigen::Vector3d::Zero());
 
template<template<class...> class MapType, class KeyType, class... RestTypes>
std::vector<std::shared_ptr<Contact>> getContactVecFromMap(
    const MapType<KeyType, std::shared_ptr<Contact>, RestTypes...> & contactList);
} // namespace ForceColl
 
#include <ForceColl/Contact.hpp>