Tesseract
Motion Planning Environment
Loading...
Searching...
No Matches
continuous_contact_manager.h
Go to the documentation of this file.
1
28#ifndef TESSERACT_COLLISION_CONTINUOUS_CONTACT_MANAGER_H
29#define TESSERACT_COLLISION_CONTINUOUS_CONTACT_MANAGER_H
30
33#include <memory>
35
37
38namespace tesseract_collision
39{
41{
42public:
43 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44
45 using Ptr = std::shared_ptr<ContinuousContactManager>;
46 using ConstPtr = std::shared_ptr<const ContinuousContactManager>;
47 using UPtr = std::unique_ptr<ContinuousContactManager>;
48 using ConstUPtr = std::unique_ptr<const ContinuousContactManager>;
49
51 virtual ~ContinuousContactManager() = default;
56
61 virtual std::string getName() const = 0;
62
70
88 virtual bool addCollisionObject(const std::string& name,
89 const int& mask_id,
90 const CollisionShapesConst& shapes,
91 const tesseract_common::VectorIsometry3d& shape_poses,
92 bool enabled = true) = 0;
93
99 virtual const CollisionShapesConst& getCollisionObjectGeometries(const std::string& name) const = 0;
100
107 getCollisionObjectGeometriesTransforms(const std::string& name) const = 0;
108
114 virtual bool hasCollisionObject(const std::string& name) const = 0;
115
121 virtual bool removeCollisionObject(const std::string& name) = 0;
122
127 virtual bool enableCollisionObject(const std::string& name) = 0;
128
133 virtual bool disableCollisionObject(const std::string& name) = 0;
134
140 virtual bool isCollisionObjectEnabled(const std::string& name) const = 0;
141
147 virtual void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) = 0;
148
154 virtual void setCollisionObjectsTransform(const std::vector<std::string>& names,
155 const tesseract_common::VectorIsometry3d& poses) = 0;
156
162
173 virtual void setCollisionObjectsTransform(const std::string& name,
174 const Eigen::Isometry3d& pose1,
175 const Eigen::Isometry3d& pose2) = 0;
176
187 virtual void setCollisionObjectsTransform(const std::vector<std::string>& names,
190
202
207 virtual const std::vector<std::string>& getCollisionObjects() const = 0;
208
213 virtual void setActiveCollisionObjects(const std::vector<std::string>& names) = 0;
214
219 virtual const std::vector<std::string>& getActiveCollisionObjects() const = 0;
220
226 virtual void
228 CollisionMarginOverrideType override_type = CollisionMarginOverrideType::REPLACE) = 0;
229
234 virtual void setDefaultCollisionMarginData(double default_collision_margin) = 0;
235
246 virtual void setPairCollisionMarginData(const std::string& name1,
247 const std::string& name2,
248 double collision_margin) = 0;
249
254 virtual const CollisionMarginData& getCollisionMarginData() const = 0;
255
258
261
267 virtual void contactTest(ContactResultMap& collisions, const ContactRequest& request) = 0;
268
274};
275
276} // namespace tesseract_collision
277
278#endif // TESSERACT_COLLISION_CONTINUOUS_CONTACT_MANAGER_H
This structure hold contact results for link pairs.
Definition: types.h:154
Definition: continuous_contact_manager.h:41
virtual bool isCollisionObjectEnabled(const std::string &name) const =0
Check if collision object is enabled.
virtual void setActiveCollisionObjects(const std::vector< std::string > &names)=0
Set which collision objects can move.
virtual const std::vector< std::string > & getCollisionObjects() const =0
Get all collision objects.
ContinuousContactManager(const ContinuousContactManager &)=delete
virtual ContinuousContactManager::UPtr clone() const =0
Clone the manager.
virtual void setDefaultCollisionMarginData(double default_collision_margin)=0
Set the default collision margin.
virtual bool hasCollisionObject(const std::string &name) const =0
Find if a collision object already exists.
ContinuousContactManager(ContinuousContactManager &&)=delete
virtual void setCollisionObjectsTransform(const std::string &name, const Eigen::Isometry3d &pose)=0
Set a single static collision object's tansforms.
virtual void setCollisionObjectsTransform(const tesseract_common::TransformMap &transforms)=0
Set a series of static collision object's tranforms.
std::shared_ptr< ContinuousContactManager > Ptr
Definition: continuous_contact_manager.h:45
virtual void setPairCollisionMarginData(const std::string &name1, const std::string &name2, double collision_margin)=0
Set the margin for a given contact pair.
virtual std::string getName() const =0
Get the name of the contact manager.
virtual void setCollisionObjectsTransform(const std::string &name, const Eigen::Isometry3d &pose1, const Eigen::Isometry3d &pose2)=0
Set a single cast(moving) collision object's tansforms.
ContinuousContactManager & operator=(ContinuousContactManager &&)=delete
virtual void applyContactManagerConfig(const ContactManagerConfig &config)
Applies settings in the config.
Definition: continuous_contact_manager.cpp:32
virtual bool addCollisionObject(const std::string &name, const int &mask_id, const CollisionShapesConst &shapes, const tesseract_common::VectorIsometry3d &shape_poses, bool enabled=true)=0
Add a collision object to the checker.
virtual void setIsContactAllowedFn(IsContactAllowedFn fn)=0
Set the active function for determining if two links are allowed to be in collision.
virtual const CollisionShapesConst & getCollisionObjectGeometries(const std::string &name) const =0
Get a collision objects collision geometries.
virtual void setCollisionObjectsTransform(const tesseract_common::TransformMap &pose1, const tesseract_common::TransformMap &pose2)=0
Set a series of cast(moving) collision object's tranforms.
virtual bool disableCollisionObject(const std::string &name)=0
Disable an object.
virtual void setCollisionMarginData(CollisionMarginData collision_margin_data, CollisionMarginOverrideType override_type=CollisionMarginOverrideType::REPLACE)=0
Set the contact distance thresholds for which collision should be considered on a per pair basis.
virtual bool removeCollisionObject(const std::string &name)=0
Remove an object from the checker.
std::shared_ptr< const ContinuousContactManager > ConstPtr
Definition: continuous_contact_manager.h:46
virtual const std::vector< std::string > & getActiveCollisionObjects() const =0
Get which collision objects can move.
std::unique_ptr< const ContinuousContactManager > ConstUPtr
Definition: continuous_contact_manager.h:48
virtual bool enableCollisionObject(const std::string &name)=0
Enable an object.
virtual const tesseract_common::VectorIsometry3d & getCollisionObjectGeometriesTransforms(const std::string &name) const =0
Get a collision objects collision geometries transforms.
virtual const CollisionMarginData & getCollisionMarginData() const =0
Get the contact distance threshold.
virtual IsContactAllowedFn getIsContactAllowedFn() const =0
Get the active function for determining if two links are allowed to be in collision.
ContinuousContactManager & operator=(const ContinuousContactManager &)=delete
virtual void setCollisionObjectsTransform(const std::vector< std::string > &names, const tesseract_common::VectorIsometry3d &pose1, const tesseract_common::VectorIsometry3d &pose2)=0
Set a series of cast(moving) collision object's tranforms.
std::unique_ptr< ContinuousContactManager > UPtr
Definition: continuous_contact_manager.h:47
virtual void contactTest(ContactResultMap &collisions, const ContactRequest &request)=0
Perform a contact test for all objects based.
virtual void setCollisionObjectsTransform(const std::vector< std::string > &names, const tesseract_common::VectorIsometry3d &poses)=0
Set a series of static collision object's tranforms.
Stores information about how the margins allowed between collision objects.
Definition: collision_margin_data.h:74
tesseract_collision::CollisionCheckConfig config(5, request, tesseract_collision::CollisionEvaluatorType::LVS_DISCRETE, 0.5)
Definition: tesseract_common_serialization_unit.cpp:154
Common Tesseract Macros.
#define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
Definition: macros.h:71
Definition: create_convex_hull.cpp:36
Definition: bullet_cast_bvh_manager.h:49
std::vector< tesseract_geometry::Geometry::ConstPtr > CollisionShapesConst
Definition: types.h:47
std::function< bool(const std::string &, const std::string &)> IsContactAllowedFn
Should return true if contact allowed, otherwise false.
Definition: types.h:59
AlignedMap< std::string, Eigen::Isometry3d > TransformMap
Definition: types.h:66
AlignedVector< Eigen::Isometry3d > VectorIsometry3d
Definition: types.h:62
CollisionMarginOverrideType
Identifies how the provided contact margin data should be applied.
Definition: collision_margin_data.h:46
Contains parameters used to configure a contact manager before a series of contact checks.
Definition: types.h:394
The ContactRequest struct.
Definition: types.h:294
Tesseracts Collision Common Types.
auto pose2
Definition: tesseract_environment_collision.cpp:155
auto pose
Definition: tesseract_environment_collision.cpp:118
auto pose1
Definition: tesseract_environment_collision.cpp:153