Contact.h

Go to the documentation of this file.

 
#pragma once
 
#include <tvm/api.h>
#include <tvm/defs.h>
 
#include <tvm/robot/Frame.h>
 
namespace tvm
{
 
namespace robot
{
 
class TVM_DLLAPI Contact : public graph::abstract::Node<Contact>
{
public:
  struct Id
  {
    std::string r1;
    std::string f1;
    std::string r2;
    std::string f2;
    int ambiguityId;
    inline bool operator<(const Id & rhs) const
    {
      // clang-format off
        return r1 < rhs.r1 ||
          ( r1 == rhs.r1 && f1 < rhs.f1 ) ||
          ( r1 == rhs.r1 && f1 == rhs.f1 && r2 < rhs.r2 ) ||
          ( r1 == rhs.r1 && f1 == rhs.f1 && r2 == rhs.r2 && f2 < rhs.f2 ) ||
          ( r1 == rhs.r1 && f1 == rhs.f1 && r2 == rhs.f2 && f2 == rhs.f2 && ambiguityId < rhs.ambiguityId );
      // clang-format on
    }
    inline bool operator==(const Id & rhs) const
    { return (r1 == rhs.r1 && f1 == rhs.f1 && r2 == rhs.r2 && f2 == rhs.f2 && ambiguityId == rhs.ambiguityId); }
  };
 
  struct View
  {
    const Id & id;
    const FramePtr & f;
    const std::vector<sva::PTransformd> & points;
  };
 
  SET_OUTPUTS(Contact,
              F1Position,
              F1Jacobian,
              F1Velocity,
              F1NormalAcceleration,
              F2Position,
              F2Jacobian,
              F2Velocity,
              F2NormalAcceleration)
 
  
  Contact(FramePtr f1, FramePtr f2, std::vector<sva::PTransformd> points, int ambiguityId = 0);
 
  inline const Frame & f1() const { return *f1_; }
 
  inline const Frame & f2() const { return *f2_; }
 
  inline const std::vector<sva::PTransformd> & f1Points() const { return f1Points_; }
 
  inline const sva::PTransformd & X_f1_f2() const { return X_f1_f2_; }
 
  inline const sva::PTransformd & X_f2_f1() const { return X_f2_f1_; }
 
  inline const std::vector<sva::PTransformd> & f2Points() const { return f2Points_; }
 
  inline const View f1View() const { return {id_, f1_, f1Points_}; };
 
  inline const View f2View() const { return {id_, f2_, f2Points_}; };
 
  inline const Id & id() const { return id_; }
 
private:
  FramePtr f1_;
  FramePtr f2_;
  sva::PTransformd X_f1_f2_;
  sva::PTransformd X_f2_f1_;
  std::vector<sva::PTransformd> f1Points_;
  std::vector<sva::PTransformd> f2Points_;
  Id id_;
};
 
using ContactPtr = std::shared_ptr<Contact>;
} // namespace robot
 
} // namespace tvm
 
std::ostream & operator<<(std::ostream & os, const tvm::robot::Contact::Id & c);