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>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_common/fwd.h>
#include <tesseract_common/any_poly.h>
#include <tesseract_common/eigen_types.h>
#include <tesseract_common/collision_margin_data.h>
#include <tesseract_geometry/fwd.h>
 
namespace tesseract_collision
{
using CollisionShapeConstPtr = std::shared_ptr<const tesseract_geometry::Geometry>;
using CollisionShapePtr = std::shared_ptr<tesseract_geometry::Geometry>;
using CollisionShapesConst = std::vector<CollisionShapeConstPtr>;
using CollisionMarginData = tesseract_common::CollisionMarginData;
using CollisionMarginPairData = tesseract_common::CollisionMarginPairData;
using CollisionMarginPairOverrideType = tesseract_common::CollisionMarginPairOverrideType;
 
class ContactResultValidator;
 
enum class ContinuousCollisionType : std::uint8_t
{
  CCType_None,
  CCType_Time0,
  CCType_Time1,
  CCType_Between
};
 
enum class ContactTestType : std::uint8_t
{
  FIRST = 0,   
  CLOSEST = 1, 
  ALL = 2,     
  LIMITED = 3  
};
 
static const std::vector<std::string> ContactTestTypeStrings = {
  "FIRST",
  "CLOSEST",
  "ALL",
  "LIMITED",
};
 
struct ContactResult
{
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  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();
 
  bool operator==(const ContactResult& rhs) const;
  bool operator!=(const ContactResult& rhs) const;
};
 
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,
                                       long sub_segment_index,
                                       long 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 shrinkToFit();
 
  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;
 
  bool operator==(const ContactResultMap& rhs) const;
  bool operator!=(const ContactResultMap& rhs) const;
 
  std::string getSummary() const;
 
private:
  ContainerType data_;
  long count_{ 0 };
};
 
struct ContactRequest
{
  ContactTestType type = ContactTestType::ALL;
 
  bool calculate_penetration = true;
 
  bool calculate_distance = true;
 
  long contact_limit = 0;
 
  std::shared_ptr<const ContactResultValidator> is_valid;
 
  ContactRequest(ContactTestType type = ContactTestType::ALL);
 
  bool operator==(const ContactRequest& rhs) const;
  bool operator!=(const ContactRequest& rhs) const;
};
 
struct ContactTestData
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  ContactTestData() = default;
  ContactTestData(const std::vector<std::string>& active,
                  CollisionMarginData collision_margin_data,
                  std::shared_ptr<const tesseract_common::ContactAllowedValidator> validator,
                  ContactRequest req,
                  ContactResultMap& res);
 
  const std::vector<std::string>* active = nullptr;
 
  CollisionMarginData collision_margin_data{ 0 };
 
  std::shared_ptr<const tesseract_common::ContactAllowedValidator> validator;
 
  ContactRequest req;
 
  ContactResultMap* res = nullptr;
 
  bool done = false;
};
 
enum class CollisionEvaluatorType : std::uint8_t
{
  NONE,
  DISCRETE,
  LVS_DISCRETE,
  CONTINUOUS,
  LVS_CONTINUOUS
};
 
enum class CollisionCheckProgramType : std::uint8_t
{
  ALL,
  ALL_EXCEPT_START,
  ALL_EXCEPT_END,
  START_ONLY,
  END_ONLY,
  INTERMEDIATE_ONLY
};
 
enum class ACMOverrideType : std::uint8_t
{
  NONE,
  ASSIGN,
  AND,
  OR,
};
 
struct ContactManagerConfig
{
  ContactManagerConfig() = default;
  ContactManagerConfig(double default_margin);
 
  std::optional<double> default_margin;
 
  CollisionMarginPairOverrideType pair_margin_override_type{ CollisionMarginPairOverrideType::NONE };
  CollisionMarginPairData pair_margin_data;
 
  tesseract_common::AllowedCollisionMatrix acm;
  ACMOverrideType acm_override_type{ ACMOverrideType::NONE };
 
  std::unordered_map<std::string, bool> modify_object_enabled;
 
  void incrementMargins(double increment);
 
  void scaleMargins(double scale);
 
  void validate() const;
 
  bool operator==(const ContactManagerConfig& rhs) const;
  bool operator!=(const ContactManagerConfig& rhs) const;
};
 
struct CollisionCheckConfig
{
  CollisionCheckConfig(ContactRequest request = ContactRequest(),
                       CollisionEvaluatorType type = CollisionEvaluatorType::DISCRETE,
                       double longest_valid_segment_length = 0.005,
                       CollisionCheckProgramType check_program_mode = CollisionCheckProgramType::ALL);
 
  ContactRequest contact_request;
 
  CollisionEvaluatorType type{ CollisionEvaluatorType::DISCRETE };
 
  double longest_valid_segment_length{ 0.005 };
 
  CollisionCheckProgramType check_program_mode{ CollisionCheckProgramType::ALL };
 
  bool operator==(const CollisionCheckConfig& rhs) const;
  bool operator!=(const CollisionCheckConfig& rhs) const;
};
 
struct ContactTrajectorySubstepResults
{
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  ContactTrajectorySubstepResults() = default;
  ContactTrajectorySubstepResults(int substep, const Eigen::VectorXd& start_state, const Eigen::VectorXd& end_state);
  ContactTrajectorySubstepResults(int substep, const Eigen::VectorXd& state);
 
  using UPtr = std::unique_ptr<ContactTrajectorySubstepResults>;
 
  int numContacts() const;
 
  tesseract_collision::ContactResultVector worstCollision() const;
 
  tesseract_collision::ContactResultMap contacts;
  int substep = -1;
  Eigen::VectorXd state0;
  Eigen::VectorXd state1;
};
 
struct ContactTrajectoryStepResults
{
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  ContactTrajectoryStepResults() = default;
  ContactTrajectoryStepResults(int step_number,
                               const Eigen::VectorXd& start_state,
                               const Eigen::VectorXd& end_state,
                               int num_substeps);
  ContactTrajectoryStepResults(int step_number, const Eigen::VectorXd& state);
 
  using UPtr = std::unique_ptr<ContactTrajectoryStepResults>;
 
  void resize(int num_substeps);
 
  int numSubsteps() const;
 
  int numContacts() const;
 
  ContactTrajectorySubstepResults worstSubstep() const;
 
  tesseract_collision::ContactResultVector worstCollision() const;
 
  ContactTrajectorySubstepResults mostCollisionsSubstep() const;
 
  std::vector<ContactTrajectorySubstepResults> substeps;
  int step = -1;
  Eigen::VectorXd state0;
  Eigen::VectorXd state1;
  int total_substeps = 0;
};
 
struct ContactTrajectoryResults
{
  // LCOV_EXCL_START
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // LCOV_EXCL_STOP
 
  ContactTrajectoryResults() = default;
  ContactTrajectoryResults(std::vector<std::string> j_names);
  ContactTrajectoryResults(std::vector<std::string> j_names, int num_steps);
 
  using UPtr = std::unique_ptr<ContactTrajectoryResults>;
 
  void resize(int num_steps);
 
  int numSteps() const;
 
  int numContacts() const;
 
  ContactTrajectoryStepResults worstStep() const;
 
  tesseract_collision::ContactResultVector worstCollision() const;
 
  ContactTrajectoryStepResults mostCollisionsStep() const;
 
  std::stringstream trajectoryCollisionResultsTable() const;
 
  std::stringstream collisionFrequencyPerLink() const;
 
  std::vector<ContactTrajectoryStepResults> steps;
  std::vector<std::string> joint_names;
  int total_steps = 0;
};
 
using ContactResultAnyPoly = tesseract_common::AnyWrapper<ContactResult>;
using ContactResultMapAnyPoly = tesseract_common::AnyWrapper<ContactResultMap>;
using ContactResultMapVectorAnyPoly = tesseract_common::AnyWrapper<std::vector<tesseract_collision::ContactResultMap>>;
 
}  // namespace tesseract_collision
 
#endif  // TESSERACT_COLLISION_TYPES_H