Contact.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 *
 * This file is inspired by Stephane's Caron original implementation as part of
 * lipm_walking_controller <https://github.com/stephane-caron/lipm_walking_controller>
 */
 
#pragma once
 
#include <mc_rbdyn/Robot.h>
#include <mc_rtc/Configuration.h>
#include <mc_rtc/logging.h>
#include <mc_tasks/api.h>
 
#include <SpaceVecAlg/SpaceVecAlg>
 
#include <Eigen/StdVector>
 
#include <cmath>
 
namespace mc_tasks
{
namespace lipm_stabilizer
{
 
using HrepXd = std::pair<Eigen::MatrixXd, Eigen::VectorXd>;
 
enum class MC_TASKS_DLLAPI ContactState
{
  Left,
  Right
};
 
namespace internal
{
 
struct MC_TASKS_DLLAPI Contact
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  Contact() = default;
 
  Contact(const mc_rbdyn::Robot & robot, const std::string & surfaceName, double friction);
 
  Contact(const mc_rbdyn::Robot & robot,
          const std::string & surfaceName,
          const sva::PTransformd & surfacePose,
          double friction);
 
  HrepXd hrep(const Eigen::Vector3d & vertical) const;
 
  const Eigen::Matrix<double, 16, 6> & wrenchFaceMatrix() const { return wrenchFaceMatrix_; }
 
  Eigen::Vector3d sagittal() const { return surfacePose_.rotation().row(0); }
 
  Eigen::Vector3d lateral() const { return surfacePose_.rotation().row(1); }
 
  Eigen::Vector3d normal() const { return surfacePose_.rotation().row(2); }
  const Eigen::Vector3d & position() const { return surfacePose_.translation(); }
 
  double halfWidth() const { return halfWidth_; }
 
  double halfLength() const { return halfLength_; }
 
  double friction() const { return friction_; }
 
  const sva::PTransformd & anklePose() const { return anklePose_; }
 
  Eigen::Vector3d sagital() { return surfacePose_.rotation().row(0); }
 
  Eigen::Vector3d lateral() { return surfacePose_.rotation().row(1); }
 
  Eigen::Vector3d vertical() { return surfacePose_.rotation().row(2); }
 
  const std::string & surfaceName() const { return surfaceName_; }
 
  const sva::PTransformd & surfacePose() const { return surfacePose_; }
 
  const std::vector<Eigen::Vector3d> & polygon() const { return contactPolygon_; }
 
  double xmin() const { return xyMin_.x(); }
 
  double xmax() const { return xyMax_.x(); }
 
  double ymin() const { return xyMin_.y(); }
 
  double ymax() const { return xyMax_.y(); }
 
private:
  void findSurfaceBoundaries(const mc_rbdyn::Surface & surface);
 
private:
  std::string surfaceName_; 
  sva::PTransformd anklePose_; 
  sva::PTransformd surfacePose_; 
  double halfLength_ = 0.; 
  double halfWidth_ = 0.; 
  double friction_ = 0.7; 
  Eigen::Matrix<double, 16, 6> wrenchFaceMatrix_; 
  Eigen::Matrix<double, 12, 6>
      wrenchFaceMatrixCoPFree_; 
  std::vector<Eigen::Vector3d>
      contactPolygon_; 
  Eigen::Vector2d xyMin_ = Eigen::Vector2d::Zero(); 
  Eigen::Vector2d xyMax_ = Eigen::Vector2d::Zero(); 
};
 
} // namespace internal
 
struct MC_TASKS_DLLAPI ContactDescription
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  ContactState first;
  internal::Contact second;
};
 
using ContactDescriptionVector = std::vector<ContactDescription, Eigen::aligned_allocator<ContactDescription>>;
 
} // namespace lipm_stabilizer
} // namespace mc_tasks
 
namespace mc_rtc
{
 
template<>
struct ConfigurationLoader<mc_tasks::lipm_stabilizer::ContactState>
{
  static mc_tasks::lipm_stabilizer::ContactState load(const mc_rtc::Configuration & config)
  {
    using ContactState = mc_tasks::lipm_stabilizer::ContactState;
    const std::string & s = config;
    if(s == "Left") { return ContactState::Left; }
    else if(s == "Right") { return ContactState::Right; }
    else { mc_rtc::log::error_and_throw("ContactState should be one of [Left, Right], {} requested.", s); }
  }
 
  static mc_rtc::Configuration save(const mc_tasks::lipm_stabilizer::ContactState & contact)
  {
    using ContactState = mc_tasks::lipm_stabilizer::ContactState;
    mc_rtc::Configuration config;
    switch(contact)
    {
      case ContactState::Left:
        config = "Left";
        break;
      case ContactState::Right:
        config = "Right";
        break;
    }
    return config;
  }
};
 
} // namespace mc_rtc