bullet_cast_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 /* Author: Levi Armstrong, Jens Petit */
33 
35 #include <map>
36 #include <utility>
37 
39 {
40 BulletCastBVHManagerPtr BulletCastBVHManager::clone() const
41 {
42  BulletCastBVHManagerPtr manager(new BulletCastBVHManager());
43 
44  for (const std::pair<const std::string, collision_detection_bullet::CollisionObjectWrapperPtr>& cow : link2cow_)
45  {
46  CollisionObjectWrapperPtr new_cow = cow.second->clone();
47 
48  assert(new_cow->getCollisionShape());
49  assert(new_cow->getCollisionShape()->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE);
50 
51  new_cow->setWorldTransform(cow.second->getWorldTransform());
52  new_cow->setContactProcessingThreshold(static_cast<btScalar>(contact_distance_));
53  manager->addCollisionObject(new_cow);
54  }
55 
56  manager->setActiveCollisionObjects(active_);
57  manager->setContactDistanceThreshold(contact_distance_);
58 
59  return manager;
60 }
61 
62 void BulletCastBVHManager::setCastCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose1,
63  const Eigen::Isometry3d& pose2)
64 {
65  // TODO: Find a way to remove this check. Need to store information in Tesseract EnvState indicating transforms with
66  // geometry
67  auto it = link2cow_.find(name);
68  if (it != link2cow_.end())
69  {
70  CollisionObjectWrapperPtr& cow = it->second;
71  assert(cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter);
72 
73  btTransform tf1 = convertEigenToBt(pose1);
74  btTransform tf2 = convertEigenToBt(pose2);
75 
76  cow->setWorldTransform(tf1);
77  link2cow_[name]->setWorldTransform(tf1);
78 
79  // If collision object is disabled dont proceed
80  if (cow->m_enabled)
81  {
82  if (btBroadphaseProxy::isConvex(cow->getCollisionShape()->getShapeType()))
83  {
84  static_cast<CastHullShape*>(cow->getCollisionShape())->updateCastTransform(tf1.inverseTimes(tf2));
85  }
86  else if (btBroadphaseProxy::isCompound(cow->getCollisionShape()->getShapeType()))
87  {
88  btCompoundShape* compound = static_cast<btCompoundShape*>(cow->getCollisionShape());
89 
90  for (int i = 0; i < compound->getNumChildShapes(); ++i)
91  {
92  if (btBroadphaseProxy::isConvex(compound->getChildShape(i)->getShapeType()))
93  {
94  const btTransform& local_tf = compound->getChildTransform(i);
95 
96  btTransform delta_tf = (tf1 * local_tf).inverseTimes(tf2 * local_tf);
97  static_cast<CastHullShape*>(compound->getChildShape(i))->updateCastTransform(delta_tf);
98  compound->updateChildTransform(i, local_tf, false); // This is required to update the BVH tree
99  }
100  else if (btBroadphaseProxy::isCompound(compound->getChildShape(i)->getShapeType()))
101  {
102  btCompoundShape* second_compound = static_cast<btCompoundShape*>(compound->getChildShape(i));
103 
104  for (int j = 0; j < second_compound->getNumChildShapes(); ++j)
105  {
106  assert(!btBroadphaseProxy::isCompound(second_compound->getChildShape(j)->getShapeType()));
107  const btTransform& local_tf = second_compound->getChildTransform(j);
108 
109  btTransform delta_tf = (tf1 * local_tf).inverseTimes(tf2 * local_tf);
110  static_cast<CastHullShape*>(second_compound->getChildShape(j))->updateCastTransform(delta_tf);
111  second_compound->updateChildTransform(j, local_tf, false); // This is required to update the BVH tree
112  }
113  second_compound->recalculateLocalAabb();
114  }
115  }
116  compound->recalculateLocalAabb();
117  }
118  else
119  {
120  ROS_ERROR_NAMED("collision_detection.bullet", "I can only continuous collision check convex shapes and "
121  "compound shapes made of convex shapes");
122  throw std::runtime_error(
123  "I can only continuous collision check convex shapes and compound shapes made of convex shapes");
124  }
125 
126  // Now update Broadphase AABB (See BulletWorld updateSingleAabb function)
128  }
129  }
130 }
131 
134  const collision_detection::AllowedCollisionMatrix* acm, bool /*self*/ = false)
135 {
136  ContactTestData cdata(active_, contact_distance_, collisions, req);
137  broadphase_->calculateOverlappingPairs(dispatcher_.get());
138  btOverlappingPairCache* pair_cache = broadphase_->getOverlappingPairCache();
139 
140  ROS_DEBUG_STREAM_NAMED("collision_detection.bullet",
141  "Number overlapping candidates " << pair_cache->getNumOverlappingPairs());
142 
143  BroadphaseContactResultCallback cc(cdata, contact_distance_, acm, false, true);
144  TesseractCollisionPairCallback collision_callback(dispatch_info_, dispatcher_.get(), cc);
145  pair_cache->processAllOverlappingPairs(&collision_callback, dispatcher_.get());
146 }
147 
148 void BulletCastBVHManager::addCollisionObject(const CollisionObjectWrapperPtr& cow)
149 {
150  std::string name = cow->getName();
151  if (cow->m_collisionFilterGroup == btBroadphaseProxy::KinematicFilter)
152  {
153  CollisionObjectWrapperPtr cast_cow = makeCastCollisionObject(cow);
154  link2cow_[name] = cast_cow;
155  }
156  else
157  {
158  link2cow_[name] = cow;
159  }
160 
161  btVector3 aabb_min, aabb_max;
162  link2cow_[name]->getAABB(aabb_min, aabb_max);
163 
164  int type = link2cow_[name]->getCollisionShape()->getShapeType();
165  link2cow_[name]->setBroadphaseHandle(
166  broadphase_->createProxy(aabb_min, aabb_max, type, link2cow_[name].get(), link2cow_[name]->m_collisionFilterGroup,
167  link2cow_[name]->m_collisionFilterMask, dispatcher_.get()));
168 }
169 
170 } // namespace collision_detection_bullet
collision_detection_bullet::CastHullShape
Casted collision shape used for checking if an object is collision free between two discrete poses.
Definition: bullet_utils.h:304
collision_detection_bullet
Definition: basic_types.h:34
collision_detection_bullet::BulletCastBVHManager::setCastCollisionObjectsTransform
void setCastCollisionObjectsTransform(const std::string &name, const Eigen::Isometry3d &pose1, const Eigen::Isometry3d &pose2)
Set a single cast (moving) collision object's tansforms.
Definition: bullet_cast_bvh_manager.cpp:91
collision_detection_bullet::BulletCastBVHManager::BulletCastBVHManager
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BulletCastBVHManager()=default
Constructor.
bullet_cast_bvh_manager.h
collision_detection_bullet::BulletBVHManager::dispatcher_
std::unique_ptr< btCollisionDispatcher > dispatcher_
The bullet collision dispatcher used for getting object to object collison algorithm.
Definition: bullet_bvh_manager.h:172
collision_detection_bullet::BulletBVHManager::broadphase_
std::unique_ptr< btBroadphaseInterface > broadphase_
The bullet broadphase interface.
Definition: bullet_bvh_manager.h:181
collision_detection_bullet::BulletCastBVHManager::clone
BulletCastBVHManagerPtr clone() const
Clone the manager.
Definition: bullet_cast_bvh_manager.cpp:69
ROS_DEBUG_STREAM_NAMED
#define ROS_DEBUG_STREAM_NAMED(name, args)
collision_detection_bullet::BulletBVHManager::active_
std::vector< std::string > active_
A list of the active collision links.
Definition: bullet_bvh_manager.h:166
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:216
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Definition: collision_matrix.h:112
name
std::string name
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:177
collision_detection_bullet::convertEigenToBt
btVector3 convertEigenToBt(const Eigen::Vector3d &v)
Converts eigen vector to bullet vector.
Definition: bullet_utils.h:129
collision_detection_bullet::BulletBVHManager::link2cow_
std::map< std::string, CollisionObjectWrapperPtr > link2cow_
A map of collision objects being managed.
Definition: bullet_bvh_manager.h:163
collision_detection_bullet::BulletCastBVHManager::addCollisionObject
void addCollisionObject(const CollisionObjectWrapperPtr &cow) override
Add a tesseract collision object to the manager.
Definition: bullet_cast_bvh_manager.cpp:177
collision_detection_bullet::makeCastCollisionObject
CollisionObjectWrapperPtr makeCastCollisionObject(const CollisionObjectWrapperPtr &cow)
Definition: bullet_utils.h:818
collision_detection_bullet::updateBroadphaseAABB
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:916
tf2
collision_detection_bullet::BulletBVHManager::dispatch_info_
btDispatcherInfo dispatch_info_
The bullet collision dispatcher configuration information.
Definition: bullet_bvh_manager.h:175
collision_detection_bullet::BulletCastBVHManager::contactTest
void contactTest(collision_detection::CollisionResult &collisions, const collision_detection::CollisionRequest &req, const collision_detection::AllowedCollisionMatrix *acm, bool self) override
Perform a contact test for all objects.
Definition: bullet_cast_bvh_manager.cpp:161
collision_detection_bullet::BulletBVHManager::contact_distance_
double contact_distance_
The contact distance threshold.
Definition: bullet_bvh_manager.h:169


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat May 21 2022 02:24:55