types.h

Go to the documentation of this file.

 
#ifndef TESSERACT_COLLISION_TYPES_H
#define TESSERACT_COLLISION_TYPES_H
 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <vector>
#include <memory>
#include <map>
#include <array>
#include <unordered_map>
#include <functional>
#include <tesseract_geometry/geometry.h>
#include <tesseract_common/types.h>
#include <tesseract_common/collision_margin_data.h>
#include <tesseract_common/allowed_collision_matrix.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
namespace tesseract_collision
{
using CollisionShapesConst = std::vector<tesseract_geometry::Geometry::ConstPtr>;
using CollisionShapeConstPtr = tesseract_geometry::Geometry::ConstPtr;
using CollisionShapePtr = tesseract_geometry::Geometry::Ptr;
using CollisionMarginData = tesseract_common::CollisionMarginData;
using CollisionMarginOverrideType = tesseract_common::CollisionMarginOverrideType;
using PairsCollisionMarginData = tesseract_common::PairsCollisionMarginData;
 
using IsContactAllowedFn = std::function<bool(const std::string&, const std::string&)>;
 
enum class ContinuousCollisionType
{
  CCType_None,
  CCType_Time0,
  CCType_Time1,
  CCType_Between
};
 
enum class ContactTestType
{
  FIRST = 0,   
  CLOSEST = 1, 
  ALL = 2,     
  LIMITED = 3  
};
 
static const std::vector<std::string> ContactTestTypeStrings = {
  "FIRST",
  "CLOSEST",
  "ALL",
  "LIMITED",
};
 
struct ContactResult
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  double distance{ std::numeric_limits<double>::max() };
  std::array<int, 2> type_id{ 0, 0 };
  std::array<std::string, 2> link_names;
  std::array<int, 2> shape_id{ -1, -1 };
  std::array<int, 2> subshape_id{ -1, -1 };
  std::array<Eigen::Vector3d, 2> nearest_points{ Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero() };
  std::array<Eigen::Vector3d, 2> nearest_points_local{ Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero() };
  std::array<Eigen::Isometry3d, 2> transform{ Eigen::Isometry3d::Identity(), Eigen::Isometry3d::Identity() };
  Eigen::Vector3d normal{ Eigen::Vector3d::Zero() };
  std::array<double, 2> cc_time{ -1, -1 };
  std::array<ContinuousCollisionType, 2> cc_type{ ContinuousCollisionType::CCType_None,
                                                  ContinuousCollisionType::CCType_None };
  std::array<Eigen::Isometry3d, 2> cc_transform{ Eigen::Isometry3d::Identity(), Eigen::Isometry3d::Identity() };
 
  bool single_contact_point{ false };
 
  ContactResult() = default;
 
  void clear();
};
 
using ContactResultVector = tesseract_common::AlignedVector<ContactResult>;
 
class ContactResultMap
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using KeyType = std::pair<std::string, std::string>;
  using MappedType = ContactResultVector;
  using ContainerType = tesseract_common::AlignedMap<KeyType, MappedType>;
  using ConstReferenceType = typename tesseract_common::AlignedMap<KeyType, MappedType>::const_reference;
  using ConstIteratorType = typename tesseract_common::AlignedMap<KeyType, MappedType>::const_iterator;
  using PairType = typename std::pair<const KeyType, MappedType>;
  using FilterFn = std::function<void(PairType&)>;
 
  ContactResult& addContactResult(const KeyType& key, ContactResult result);
 
  ContactResult& addContactResult(const KeyType& key, const MappedType& results);
 
  ContactResult& setContactResult(const KeyType& key, ContactResult result);
 
  ContactResult& setContactResult(const KeyType& key, const MappedType& results);
 
  void addInterpolatedCollisionResults(ContactResultMap& sub_segment_results,
                                       int sub_segment_index,
                                       int sub_segment_last_index,
                                       const std::vector<std::string>& active_link_names,
                                       double segment_dt,
                                       bool discrete,
                                       const ContactResultMap::FilterFn& filter = nullptr);
 
  // Flatten functions
  void flattenMoveResults(ContactResultVector& v);
  void flattenCopyResults(ContactResultVector& v) const;
  void flattenWrapperResults(std::vector<std::reference_wrapper<ContactResult>>& v);
  void flattenWrapperResults(std::vector<std::reference_wrapper<const ContactResult>>& v) const;
 
  void filter(const FilterFn& filter);
 
  long count() const;
 
  std::size_t size() const;
 
  bool empty() const;
 
  void clear();
 
  void release();
 
  const ContainerType& getContainer() const;
 
  // Iterators //
 
  ConstIteratorType begin() const;
  ConstIteratorType end() const;
  ConstIteratorType cbegin() const;
  ConstIteratorType cend() const;
 
  // Element Access //
 
  const ContactResultVector& at(const KeyType& key) const;
  ConstIteratorType find(const KeyType& key) const;
 
private:
  ContainerType data_;
  long count_{ 0 };
};
 
using IsContactResultValidFn = std::function<bool(const ContactResult&)>;
 
struct ContactRequest
{
  ContactTestType type = ContactTestType::ALL;
 
  bool calculate_penetration = true;
 
  bool calculate_distance = true;
 
  long contact_limit = 0;
 
  IsContactResultValidFn is_valid = nullptr;
 
  ContactRequest(ContactTestType type = ContactTestType::ALL);
};
 
struct ContactTestData
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  ContactTestData() = default;
  ContactTestData(const std::vector<std::string>& active,
                  CollisionMarginData collision_margin_data,
                  IsContactAllowedFn fn,
                  ContactRequest req,
                  ContactResultMap& res);
 
  const std::vector<std::string>* active = nullptr;
 
  CollisionMarginData collision_margin_data{ 0 };
 
  IsContactAllowedFn fn = nullptr;
 
  ContactRequest req;
 
  ContactResultMap* res = nullptr;
 
  bool done = false;
};
 
enum class CollisionEvaluatorType
{
  NONE,
  DISCRETE,
  LVS_DISCRETE,
  CONTINUOUS,
  LVS_CONTINUOUS
};
 
enum class ACMOverrideType
{
  NONE,
  ASSIGN,
  AND,
  OR,
};
 
struct ContactManagerConfig
{
  ContactManagerConfig() = default;
  ContactManagerConfig(double default_margin);
 
  CollisionMarginOverrideType margin_data_override_type{ CollisionMarginOverrideType::NONE };
  CollisionMarginData margin_data;
 
  tesseract_common::AllowedCollisionMatrix acm;
  ACMOverrideType acm_override_type{ ACMOverrideType::OR };
 
  std::unordered_map<std::string, bool> modify_object_enabled;
};
 
struct CollisionCheckConfig
{
  CollisionCheckConfig() = default;
  CollisionCheckConfig(double default_margin,
                       ContactRequest request = ContactRequest(),
                       CollisionEvaluatorType type = CollisionEvaluatorType::DISCRETE,
                       double longest_valid_segment_length = 0.005);
 
  ContactManagerConfig contact_manager_config;
 
  ContactRequest contact_request;
  CollisionEvaluatorType type{ CollisionEvaluatorType::DISCRETE };
  double longest_valid_segment_length{ 0.005 };
};
}  // namespace tesseract_collision
 
#endif  // TESSERACT_COLLISION_TYPES_H