moveit2
The MoveIt Motion Planning Framework for ROS 2.
bullet_bvh_manager.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD-2-Clause)
3  *
4  * Copyright (c) 2017, Southwest Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
21  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
22  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
23  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
24  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
26  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
27  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
28  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  *********************************************************************/
31 
32 /* Authors: Levi Armstrong, Jens Petit */
33 
35 #include <map>
36 #include <utility>
37 
39 {
41 {
42  dispatcher_ = std::make_unique<btCollisionDispatcher>(&coll_config_);
43 
44  dispatcher_->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE, BOX_SHAPE_PROXYTYPE,
45  coll_config_.getCollisionAlgorithmCreateFunc(CONVEX_SHAPE_PROXYTYPE,
46  CONVEX_SHAPE_PROXYTYPE));
47 
48  dispatcher_->setDispatcherFlags(dispatcher_->getDispatcherFlags() &
49  ~btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD);
50 
51  broadphase_ = std::make_unique<btDbvtBroadphase>();
52 
53  broadphase_->getOverlappingPairCache()->setOverlapFilterCallback(&filter_callback_);
54 
56 }
57 
59 {
60  // clean up remaining objects
61  for (const std::pair<const std::string, collision_detection_bullet::CollisionObjectWrapperPtr>& cow : link2cow_)
63 }
64 
65 bool BulletBVHManager::hasCollisionObject(const std::string& name) const
66 {
67  return (link2cow_.find(name) != link2cow_.end());
68 }
69 
71 {
72  auto it = link2cow_.find(name);
73  if (it != link2cow_.end())
74  {
75  CollisionObjectWrapperPtr& cow = it->second;
77  link2cow_.erase(name);
78  return true;
79  }
80 
81  return false;
82 }
83 
85 {
86  auto it = link2cow_.find(name);
87  if (it != link2cow_.end())
88  {
89  it->second->m_enabled = true;
90  return true;
91  }
92 
93  return false;
94 }
95 
97 {
98  auto it = link2cow_.find(name);
99  if (it != link2cow_.end())
100  {
101  it->second->m_enabled = false;
102  return true;
103  }
104 
105  return false;
106 }
107 
108 void BulletBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose)
109 {
110  // TODO(j-petit): Find a way to remove this check. Need to store information in CollisionEnv transforms with geometry
111  auto it = link2cow_.find(name);
112  if (it != link2cow_.end())
113  {
114  CollisionObjectWrapperPtr& cow = it->second;
115  btTransform tf = convertEigenToBt(pose);
116  cow->setWorldTransform(tf);
117 
118  // Now update Broadphase AABB (See BulletWorld updateSingleAabb function)
119  if (cow->getBroadphaseHandle())
121  }
122 }
123 
124 void BulletBVHManager::setActiveCollisionObjects(const std::vector<std::string>& names)
125 {
126  active_ = names;
127 
128  // Now need to update the broadphase with correct aabb
129  for (std::pair<const std::string, CollisionObjectWrapperPtr>& co : link2cow_)
130  {
131  CollisionObjectWrapperPtr& cow = co.second;
133 
134  // The broadphase tree structure has to be updated, therefore remove and add is necessary
137  }
138 }
139 
140 const std::vector<std::string>& BulletBVHManager::getActiveCollisionObjects() const
141 {
142  return active_;
143 }
144 
146 {
147  contact_distance_ = contact_distance;
148 
149  for (std::pair<const std::string, CollisionObjectWrapperPtr>& co : link2cow_)
150  {
151  CollisionObjectWrapperPtr& cow = co.second;
152  cow->setContactProcessingThreshold(static_cast<btScalar>(contact_distance));
153  if (cow->getBroadphaseHandle())
155  }
156 }
157 
159 {
160  return contact_distance_;
161 }
162 
163 const std::map<std::string, CollisionObjectWrapperPtr>& BulletBVHManager::getCollisionObjects() const
164 {
165  return link2cow_;
166 }
167 
168 } // namespace collision_detection_bullet
bool hasCollisionObject(const std::string &name) const
Find if a collision object already exists.
std::vector< std::string > active_
A list of the active collision links.
std::unique_ptr< btBroadphaseInterface > broadphase_
The bullet broadphase interface.
double contact_distance_
The contact distance threshold.
BroadphaseFilterCallback filter_callback_
Callback function for culling objects before a broadphase check.
const std::vector< std::string > & getActiveCollisionObjects() const
Get which collision objects are active.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BulletBVHManager()
Constructor.
std::map< std::string, CollisionObjectWrapperPtr > link2cow_
A map of collision objects being managed.
bool removeCollisionObject(const std::string &name)
Remove an object from the checker.
void setCollisionObjectsTransform(const std::string &name, const Eigen::Isometry3d &pose)
Set a single static collision object's tansform.
void setContactDistanceThreshold(double contact_distance)
Set the contact distance threshold for which collision should be considered through expanding the AAB...
btDefaultCollisionConfiguration coll_config_
The bullet collision configuration.
std::unique_ptr< btCollisionDispatcher > dispatcher_
The bullet collision dispatcher used for getting object to object collision algorithm.
bool disableCollisionObject(const std::string &name)
Disable an object.
bool enableCollisionObject(const std::string &name)
Enable an object.
void setActiveCollisionObjects(const std::vector< std::string > &names)
Set which collision objects are active.
double getContactDistanceThreshold() const
Get the contact distance threshold.
const std::map< std::string, CollisionObjectWrapperPtr > & getCollisionObjects() const
btVector3 convertEigenToBt(const Eigen::Vector3d &v)
Converts eigen vector to bullet vector.
Definition: bullet_utils.h:65
void updateCollisionObjectFilters(const std::vector< std::string > &active, CollisionObjectWrapper &cow)
Update a collision objects filters.
void updateBroadphaseAABB(const CollisionObjectWrapperPtr &cow, const std::unique_ptr< btBroadphaseInterface > &broadphase, const std::unique_ptr< btCollisionDispatcher > &dispatcher)
Update the Broadphase AABB for the input collision object.
Definition: bullet_utils.h:678
void removeCollisionObjectFromBroadphase(const CollisionObjectWrapperPtr &cow, const std::unique_ptr< btBroadphaseInterface > &broadphase, const std::unique_ptr< btCollisionDispatcher > &dispatcher)
Remove the collision object from broadphase.
Definition: bullet_utils.h:693
void addCollisionObjectToBroadphase(const CollisionObjectWrapperPtr &cow, const std::unique_ptr< btBroadphaseInterface > &broadphase, const std::unique_ptr< btCollisionDispatcher > &dispatcher)
Add the collision object to broadphase.
const btScalar BULLET_DEFAULT_CONTACT_DISTANCE
Definition: bullet_utils.h:55
name
Definition: setup.py:7