Tesseract
Motion Planning Environment
Loading...
Searching...
No Matches
bullet_cast_simple_manager.h
Go to the documentation of this file.
1
41#ifndef TESSERACT_COLLISION_BULLET_CAST_SIMPLE_MANAGERS_H
42#define TESSERACT_COLLISION_BULLET_CAST_SIMPLE_MANAGERS_H
43
47
49{
52{
53public:
54 using Ptr = std::shared_ptr<BulletCastSimpleManager>;
55 using ConstPtr = std::shared_ptr<const BulletCastSimpleManager>;
56 using UPtr = std::unique_ptr<BulletCastSimpleManager>;
57 using ConstUPtr = std::unique_ptr<const BulletCastSimpleManager>;
58
59 BulletCastSimpleManager(std::string name = "BulletCastSimpleManager",
61 ~BulletCastSimpleManager() override = default;
66
67 std::string getName() const override final;
68
69 ContinuousContactManager::UPtr clone() const override final;
70
71 bool addCollisionObject(const std::string& name,
72 const int& mask_id,
73 const CollisionShapesConst& shapes,
74 const tesseract_common::VectorIsometry3d& shape_poses,
75 bool enabled = true) override final;
76
77 const CollisionShapesConst& getCollisionObjectGeometries(const std::string& name) const override final;
78
79 const tesseract_common::VectorIsometry3d&
80 getCollisionObjectGeometriesTransforms(const std::string& name) const override final;
81
82 bool hasCollisionObject(const std::string& name) const override final;
83
84 bool removeCollisionObject(const std::string& name) override final;
85
86 bool enableCollisionObject(const std::string& name) override final;
87
88 bool disableCollisionObject(const std::string& name) override final;
89
90 bool isCollisionObjectEnabled(const std::string& name) const override final;
91
92 void setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose) override final;
93
94 void setCollisionObjectsTransform(const std::vector<std::string>& names,
95 const tesseract_common::VectorIsometry3d& poses) override final;
96
97 void setCollisionObjectsTransform(const tesseract_common::TransformMap& transforms) override final;
98
99 void setCollisionObjectsTransform(const std::string& name,
100 const Eigen::Isometry3d& pose1,
101 const Eigen::Isometry3d& pose2) override final;
102
103 void setCollisionObjectsTransform(const std::vector<std::string>& names,
104 const tesseract_common::VectorIsometry3d& pose1,
105 const tesseract_common::VectorIsometry3d& pose2) override final;
106
107 void setCollisionObjectsTransform(const tesseract_common::TransformMap& pose1,
108 const tesseract_common::TransformMap& pose2) override final;
109
110 const std::vector<std::string>& getCollisionObjects() const override final;
111
112 void setActiveCollisionObjects(const std::vector<std::string>& names) override final;
113
114 const std::vector<std::string>& getActiveCollisionObjects() const override final;
115
117 CollisionMarginData collision_margin_data,
118 CollisionMarginOverrideType override_type = CollisionMarginOverrideType::REPLACE) override final;
119
120 const CollisionMarginData& getCollisionMarginData() const override final;
121
122 void setDefaultCollisionMarginData(double default_collision_margin) override final;
123
124 void setPairCollisionMarginData(const std::string& name1,
125 const std::string& name2,
126 double collision_margin) override final;
127
128 void setIsContactAllowedFn(IsContactAllowedFn fn) override final;
129
130 IsContactAllowedFn getIsContactAllowedFn() const override final;
131
132 void contactTest(ContactResultMap& collisions, const ContactRequest& request) override final;
133
138 void addCollisionObject(const COW::Ptr& cow);
139
140private:
141 std::string name_;
143 std::vector<std::string> active_;
145 std::vector<std::string> collision_objects_;
147 std::unique_ptr<btCollisionDispatcher> dispatcher_;
149 btDispatcherInfo dispatch_info_;
160
166
169};
170
171} // namespace tesseract_collision::tesseract_collision_bullet
172
173#endif // TESSERACT_COLLISION_BULLET_CAST_SIMPLE_MANAGERS_H
#define vector(a, b, c)
Definition: FloatMath.inl:3227
Tesseract ROS Bullet environment utility function.
This structure hold contact results for link pairs.
Definition: types.h:154
Definition: continuous_contact_manager.h:41
A simple implementation of a tesseract manager which does not use BHV.
Definition: bullet_cast_simple_manager.h:52
void setIsContactAllowedFn(IsContactAllowedFn fn) override final
Set the active function for determining if two links are allowed to be in collision.
Definition: bullet_cast_simple_manager.cpp:367
bool addCollisionObject(const std::string &name, const int &mask_id, const CollisionShapesConst &shapes, const tesseract_common::VectorIsometry3d &shape_poses, bool enabled=true) override final
Add a collision object to the checker.
Definition: bullet_cast_simple_manager.cpp:92
void setCollisionMarginData(CollisionMarginData collision_margin_data, CollisionMarginOverrideType override_type=CollisionMarginOverrideType::REPLACE) override final
Set the contact distance thresholds for which collision should be considered on a per pair basis.
Definition: bullet_cast_simple_manager.cpp:342
TesseractCollisionConfigurationInfo config_info_
The bullet collision configuration information.
Definition: bullet_cast_simple_manager.h:151
IsContactAllowedFn getIsContactAllowedFn() const override final
Get the active function for determining if two links are allowed to be in collision.
Definition: bullet_cast_simple_manager.cpp:368
ContinuousContactManager::UPtr clone() const override final
Clone the manager.
Definition: bullet_cast_simple_manager.cpp:66
std::unique_ptr< btCollisionDispatcher > dispatcher_
The bullet collision dispatcher used for getting object to object collison algorithm.
Definition: bullet_cast_simple_manager.h:147
bool enableCollisionObject(const std::string &name) override final
Enable an object.
Definition: bullet_cast_simple_manager.cpp:148
std::vector< std::string > active_
A list of the active collision objects.
Definition: bullet_cast_simple_manager.h:143
std::unique_ptr< const BulletCastSimpleManager > ConstUPtr
Definition: bullet_cast_simple_manager.h:57
void contactTest(ContactResultMap &collisions, const ContactRequest &request) override final
Perform a contact test for all objects based.
Definition: bullet_cast_simple_manager.cpp:369
Link2Cow link2cow_
A map of collision objects being managed.
Definition: bullet_cast_simple_manager.h:155
std::string getName() const override final
Get the name of the contact manager.
Definition: bullet_cast_simple_manager.cpp:64
bool disableCollisionObject(const std::string &name) override final
Disable an object.
Definition: bullet_cast_simple_manager.cpp:161
std::shared_ptr< const BulletCastSimpleManager > ConstPtr
Definition: bullet_cast_simple_manager.h:55
const std::vector< std::string > & getCollisionObjects() const override final
Get all collision objects.
Definition: bullet_cast_simple_manager.cpp:304
bool removeCollisionObject(const std::string &name) override final
Remove an object from the checker.
Definition: bullet_cast_simple_manager.cpp:133
bool isCollisionObjectEnabled(const std::string &name) const override final
Check if collision object is enabled.
Definition: bullet_cast_simple_manager.cpp:174
const std::vector< std::string > & getActiveCollisionObjects() const override final
Get which collision objects can move.
Definition: bullet_cast_simple_manager.cpp:340
BulletCastSimpleManager & operator=(BulletCastSimpleManager &&)=delete
bool hasCollisionObject(const std::string &name) const override final
Find if a collision object already exists.
Definition: bullet_cast_simple_manager.cpp:128
std::unique_ptr< BulletCastSimpleManager > UPtr
Definition: bullet_cast_simple_manager.h:56
TesseractCollisionConfiguration coll_config_
The bullet collision configuration.
Definition: bullet_cast_simple_manager.h:153
void setCollisionObjectsTransform(const std::string &name, const Eigen::Isometry3d &pose) override final
Set a single static collision object's tansforms.
Definition: bullet_cast_simple_manager.cpp:183
const CollisionShapesConst & getCollisionObjectGeometries(const std::string &name) const override final
Get a collision objects collision geometries.
Definition: bullet_cast_simple_manager.cpp:113
void onCollisionMarginDataChanged()
This function will update internal data when margin data has changed.
Definition: bullet_cast_simple_manager.cpp:456
BulletCastSimpleManager & operator=(const BulletCastSimpleManager &)=delete
void setDefaultCollisionMarginData(double default_collision_margin) override final
Set the default collision margin.
Definition: bullet_cast_simple_manager.cpp:349
std::shared_ptr< BulletCastSimpleManager > Ptr
Definition: bullet_cast_simple_manager.h:54
btDispatcherInfo dispatch_info_
The bullet collision dispatcher configuration information.
Definition: bullet_cast_simple_manager.h:149
std::string name_
Definition: bullet_cast_simple_manager.h:141
void setPairCollisionMarginData(const std::string &name1, const std::string &name2, double collision_margin) override final
Set the margin for a given contact pair.
Definition: bullet_cast_simple_manager.cpp:355
std::vector< std::string > collision_objects_
A list of the collision objects.
Definition: bullet_cast_simple_manager.h:145
std::vector< COW::Ptr > cows_
A vector of collision objects (active followed by static)
Definition: bullet_cast_simple_manager.h:157
Link2Cow link2castcow_
A map of cast collision objects being managed.
Definition: bullet_cast_simple_manager.h:159
ContactTestData contact_test_data_
This is used when contactTest is called. It is also added as a user point to the collsion objects so ...
Definition: bullet_cast_simple_manager.h:165
const CollisionMarginData & getCollisionMarginData() const override final
Get the contact distance threshold.
Definition: bullet_cast_simple_manager.cpp:363
const tesseract_common::VectorIsometry3d & getCollisionObjectGeometriesTransforms(const std::string &name) const override final
Get a collision objects collision geometries transforms.
Definition: bullet_cast_simple_manager.cpp:121
void setActiveCollisionObjects(const std::vector< std::string > &names) override final
Set which collision objects can move.
Definition: bullet_cast_simple_manager.cpp:306
This is a tesseract bullet collsion object.
Definition: bullet_utils.h:89
This is a modified configuration that included the modified Bullet algorithms.
Definition: tesseract_collision_configuration.h:82
Stores information about how the margins allowed between collision objects.
Definition: collision_margin_data.h:74
This is the continuous contact manager base class.
Definition: bullet_cast_bvh_manager.h:49
std::map< std::string, COW::Ptr > Link2Cow
Definition: bullet_utils.h:150
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
Definition: allowed_collision_matrix.h:16
CollisionMarginOverrideType
Identifies how the provided contact margin data should be applied.
Definition: collision_margin_data.h:46
The ContactRequest struct.
Definition: types.h:294
This data is intended only to be used internal to the collision checkers as a container and should no...
Definition: types.h:319
Modified bullet collision configuration.
auto pose2
Definition: tesseract_environment_collision.cpp:155
auto pose
Definition: tesseract_environment_collision.cpp:118
auto pose1
Definition: tesseract_environment_collision.cpp:153