bullet_utils.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  * Copyright (c) 2013, John Schulman
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
26  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *********************************************************************/
32 
33 /* Authors: John Schulman, Levi Armstrong */
34 
36 
37 #include <BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h>
38 #include <BulletCollision/CollisionShapes/btShapeHull.h>
39 #include <BulletCollision/Gimpact/btGImpactShape.h>
41 #include <memory>
42 #include <octomap/octomap.h>
43 #include <ros/console.h>
44 
46 {
47 btCollisionShape* createShapePrimitive(const shapes::Box* geom, const CollisionObjectType& collision_object_type)
48 {
49  (void)(collision_object_type);
50  assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE);
51  const double* size = geom->size;
52  btScalar a = static_cast<btScalar>(size[0] / 2);
53  btScalar b = static_cast<btScalar>(size[1] / 2);
54  btScalar c = static_cast<btScalar>(size[2] / 2);
55 
56  return (new btBoxShape(btVector3(a, b, c)));
57 }
58 
59 btCollisionShape* createShapePrimitive(const shapes::Sphere* geom, const CollisionObjectType& collision_object_type)
60 {
61  (void)(collision_object_type);
62  assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE);
63  return (new btSphereShape(static_cast<btScalar>(geom->radius)));
64 }
65 
66 btCollisionShape* createShapePrimitive(const shapes::Cylinder* geom, const CollisionObjectType& collision_object_type)
67 {
68  (void)(collision_object_type);
69  assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE);
70  btScalar r = static_cast<btScalar>(geom->radius);
71  btScalar l = static_cast<btScalar>(geom->length / 2);
72  return (new btCylinderShapeZ(btVector3(r, r, l)));
73 }
74 
75 btCollisionShape* createShapePrimitive(const shapes::Cone* geom, const CollisionObjectType& collision_object_type)
76 {
77  (void)(collision_object_type);
78  assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE);
79  btScalar r = static_cast<btScalar>(geom->radius);
80  btScalar l = static_cast<btScalar>(geom->length);
81  return (new btConeShapeZ(r, l));
82 }
83 
84 btCollisionShape* createShapePrimitive(const shapes::Mesh* geom, const CollisionObjectType& collision_object_type,
85  CollisionObjectWrapper* cow)
86 {
87  assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE ||
88  collision_object_type == CollisionObjectType::CONVEX_HULL ||
89  collision_object_type == CollisionObjectType::SDF);
90 
91  if (geom->vertex_count > 0 && geom->triangle_count > 0)
92  {
93  // convert the mesh to the assigned collision object type
94  switch (collision_object_type)
95  {
97  {
98  // Create a convex hull shape to approximate Trimesh
101  std::vector<int> faces;
102 
103  input.reserve(geom->vertex_count);
104  for (unsigned int i = 0; i < geom->vertex_count; ++i)
105  input.push_back(Eigen::Vector3d(geom->vertices[3 * i], geom->vertices[3 * i + 1], geom->vertices[3 * i + 2]));
106 
107  if (collision_detection_bullet::createConvexHull(vertices, faces, input) < 0)
108  return nullptr;
109 
110  btConvexHullShape* subshape = new btConvexHullShape();
111  for (const Eigen::Vector3d& v : vertices)
112  subshape->addPoint(
113  btVector3(static_cast<btScalar>(v[0]), static_cast<btScalar>(v[1]), static_cast<btScalar>(v[2])));
114 
115  return subshape;
116  }
118  {
119  btCompoundShape* compound =
120  new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast<int>(geom->triangle_count));
121  compound->setMargin(
122  BULLET_MARGIN); // margin: compound seems to have no effect when positive but has an effect when negative
123 
124  for (unsigned i = 0; i < geom->triangle_count; ++i)
125  {
126  btVector3 v[3];
127  for (unsigned x = 0; x < 3; ++x)
128  {
129  unsigned idx = geom->triangles[3 * i + x];
130  for (unsigned y = 0; y < 3; ++y)
131  {
132  v[x][y] = static_cast<btScalar>(geom->vertices[3 * idx + y]);
133  }
134  }
135 
136  btCollisionShape* subshape = new btTriangleShapeEx(v[0], v[1], v[2]);
137  if (subshape != nullptr)
138  {
139  cow->manage(subshape);
140  subshape->setMargin(BULLET_MARGIN);
141  btTransform geom_trans;
142  geom_trans.setIdentity();
143  compound->addChildShape(geom_trans, subshape);
144  }
145  }
146 
147  return compound;
148  }
149  default:
150  {
151  ROS_ERROR("This bullet shape type (%d) is not supported for geometry meshs",
152  static_cast<int>(collision_object_type));
153  return nullptr;
154  }
155  }
156  }
157  ROS_ERROR("The mesh is empty!");
158  return nullptr;
159 }
160 
161 btCollisionShape* createShapePrimitive(const shapes::OcTree* geom, const CollisionObjectType& collision_object_type,
162  CollisionObjectWrapper* cow)
163 {
164  assert(collision_object_type == CollisionObjectType::USE_SHAPE_TYPE ||
165  collision_object_type == CollisionObjectType::CONVEX_HULL ||
166  collision_object_type == CollisionObjectType::SDF ||
167  collision_object_type == CollisionObjectType::MULTI_SPHERE);
168 
169  btCompoundShape* subshape =
170  new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast<int>(geom->octree->size()));
171  double occupancy_threshold = geom->octree->getOccupancyThres();
172 
173  // convert the mesh to the assigned collision object type
174  switch (collision_object_type)
175  {
177  {
178  for (auto it = geom->octree->begin(static_cast<unsigned char>(geom->octree->getTreeDepth())),
179  end = geom->octree->end();
180  it != end; ++it)
181  {
182  if (it->getOccupancy() >= occupancy_threshold)
183  {
184  double size = it.getSize();
185  btTransform geom_trans;
186  geom_trans.setIdentity();
187  geom_trans.setOrigin(btVector3(static_cast<btScalar>(it.getX()), static_cast<btScalar>(it.getY()),
188  static_cast<btScalar>(it.getZ())));
189  btScalar l = static_cast<btScalar>(size / 2);
190  btBoxShape* childshape = new btBoxShape(btVector3(l, l, l));
191  childshape->setMargin(BULLET_MARGIN);
192  cow->manage(childshape);
193 
194  subshape->addChildShape(geom_trans, childshape);
195  }
196  }
197  return subshape;
198  }
200  {
201  for (auto it = geom->octree->begin(static_cast<unsigned char>(geom->octree->getTreeDepth())),
202  end = geom->octree->end();
203  it != end; ++it)
204  {
205  if (it->getOccupancy() >= occupancy_threshold)
206  {
207  double size = it.getSize();
208  btTransform geom_trans;
209  geom_trans.setIdentity();
210  geom_trans.setOrigin(btVector3(static_cast<btScalar>(it.getX()), static_cast<btScalar>(it.getY()),
211  static_cast<btScalar>(it.getZ())));
212  btSphereShape* childshape =
213  new btSphereShape(static_cast<btScalar>(std::sqrt(2 * ((size / 2) * (size / 2)))));
214  childshape->setMargin(BULLET_MARGIN);
215  cow->manage(childshape);
216 
217  subshape->addChildShape(geom_trans, childshape);
218  }
219  }
220  return subshape;
221  }
222  default:
223  {
224  ROS_ERROR("This bullet shape type (%d) is not supported for geometry octree",
225  static_cast<int>(collision_object_type));
226  return nullptr;
227  }
228  }
229 }
230 
231 btCollisionShape* createShapePrimitive(const shapes::ShapeConstPtr& geom,
232  const CollisionObjectType& collision_object_type, CollisionObjectWrapper* cow)
233 {
234  switch (geom->type)
235  {
236  case shapes::BOX:
237  {
238  return createShapePrimitive(static_cast<const shapes::Box*>(geom.get()), collision_object_type);
239  }
240  case shapes::SPHERE:
241  {
242  return createShapePrimitive(static_cast<const shapes::Sphere*>(geom.get()), collision_object_type);
243  }
244  case shapes::CYLINDER:
245  {
246  return createShapePrimitive(static_cast<const shapes::Cylinder*>(geom.get()), collision_object_type);
247  }
248  case shapes::CONE:
249  {
250  return createShapePrimitive(static_cast<const shapes::Cone*>(geom.get()), collision_object_type);
251  }
252  case shapes::MESH:
253  {
254  return createShapePrimitive(static_cast<const shapes::Mesh*>(geom.get()), collision_object_type, cow);
255  }
256  case shapes::OCTREE:
257  {
258  return createShapePrimitive(static_cast<const shapes::OcTree*>(geom.get()), collision_object_type, cow);
259  }
260  default:
261  {
262  ROS_ERROR("This geometric shape type (%d) is not supported using BULLET yet", static_cast<int>(geom->type));
263  return nullptr;
264  }
265  }
266 }
267 
269  const std::vector<shapes::ShapeConstPtr>& shapes,
270  const AlignedVector<Eigen::Isometry3d>& shape_poses,
271  const std::vector<CollisionObjectType>& collision_object_types,
272  bool active)
273  : m_name(name)
274  , m_type_id(type_id)
275  , m_shapes(shapes)
276  , m_shape_poses(shape_poses)
277  , m_collision_object_types(collision_object_types)
278 {
279  if (shapes.empty() || shape_poses.empty() ||
280  (shapes.size() != shape_poses.size() || collision_object_types.empty() ||
281  shapes.size() != collision_object_types.size()))
282  {
283  throw std::exception();
284  }
285 
286  this->setContactProcessingThreshold(BULLET_DEFAULT_CONTACT_DISTANCE);
287  assert(!name.empty());
288 
289  if (active)
290  {
291  m_collisionFilterGroup = btBroadphaseProxy::KinematicFilter;
292  m_collisionFilterMask = btBroadphaseProxy::KinematicFilter | btBroadphaseProxy::StaticFilter;
293  }
294  else
295  {
296  m_collisionFilterGroup = btBroadphaseProxy::StaticFilter;
297  m_collisionFilterMask = btBroadphaseProxy::KinematicFilter;
298  }
299 
300  if (shapes.size() == 1)
301  {
302  btCollisionShape* shape = createShapePrimitive(m_shapes[0], collision_object_types[0], this);
303  shape->setMargin(BULLET_MARGIN);
304  manage(shape);
305  setCollisionShape(shape);
306  setWorldTransform(convertEigenToBt(m_shape_poses[0]));
307  }
308  else
309  {
310  btCompoundShape* compound =
311  new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast<int>(m_shapes.size()));
312  manage(compound);
313  // margin on compound seems to have no effect when positive but has an effect when negative
314  compound->setMargin(BULLET_MARGIN);
315  setCollisionShape(compound);
316 
317  setWorldTransform(convertEigenToBt(m_shape_poses[0]));
318  Eigen::Isometry3d inv_world = m_shape_poses[0].inverse();
319 
320  for (std::size_t j = 0; j < m_shapes.size(); ++j)
321  {
322  btCollisionShape* subshape = createShapePrimitive(m_shapes[j], collision_object_types[j], this);
323  if (subshape != nullptr)
324  {
325  manage(subshape);
326  subshape->setMargin(BULLET_MARGIN);
327  btTransform geom_trans = convertEigenToBt(inv_world * m_shape_poses[j]);
328  compound->addChildShape(geom_trans, subshape);
329  }
330  }
331  }
332 }
333 
334 CollisionObjectWrapper::CollisionObjectWrapper(const std::string& name, const collision_detection::BodyType& type_id,
335  const std::vector<shapes::ShapeConstPtr>& shapes,
336  const AlignedVector<Eigen::Isometry3d>& shape_poses,
337  const std::vector<CollisionObjectType>& collision_object_types,
338  const std::set<std::string>& touch_links)
339  : CollisionObjectWrapper(name, type_id, shapes, shape_poses, collision_object_types, true)
340 {
341  m_touch_links = touch_links;
342 }
343 
345  const std::vector<shapes::ShapeConstPtr>& shapes,
346  const AlignedVector<Eigen::Isometry3d>& shape_poses,
347  const std::vector<CollisionObjectType>& collision_object_types,
348  const std::vector<std::shared_ptr<void>>& data)
349  : m_name(name)
350  , m_type_id(type_id)
351  , m_shapes(shapes)
352  , m_shape_poses(shape_poses)
353  , m_collision_object_types(collision_object_types)
354  , m_data(data)
355 {
356 }
357 } // namespace collision_detection_bullet
shapes::Cone::radius
double radius
collision_detection_bullet
Definition: basic_types.h:34
shapes::Box::size
double size[3]
shapes
shapes::Cone::length
double length
shapes::OcTree::octree
std::shared_ptr< const octomap::OcTree > octree
shapes::Mesh::triangles
unsigned int * triangles
collision_detection_bullet::CollisionObjectType::SDF
@ SDF
Use the mesh and rpresent it by a signed distance fields collision object.
collision_detection_bullet::CollisionObjectType
CollisionObjectType
Definition: basic_types.h:62
shapes::Cylinder
shapes::Mesh::triangle_count
unsigned int triangle_count
shapes::SPHERE
SPHERE
shapes::Mesh
shapes::MESH
MESH
collision_detection_bullet::CollisionObjectWrapper::CollisionObjectWrapper
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CollisionObjectWrapper(const std::string &name, const collision_detection::BodyType &type_id, const std::vector< shapes::ShapeConstPtr > &shapes, const AlignedVector< Eigen::Isometry3d > &shape_poses, const std::vector< CollisionObjectType > &collision_object_types, bool active=true)
Standard constructor.
Definition: bullet_utils.cpp:298
shapes::CONE
CONE
console.h
shapes::Sphere::radius
double radius
shapes::Box
name
std::string name
shapes::Sphere
shapes::Mesh::vertex_count
unsigned int vertex_count
y
double y
bullet_utils.h
collision_detection_bullet::convertEigenToBt
btVector3 convertEigenToBt(const Eigen::Vector3d &v)
Converts eigen vector to bullet vector.
Definition: bullet_utils.h:129
ROS_ERROR
#define ROS_ERROR(...)
shapes.h
collision_detection_bullet::BULLET_COMPOUND_USE_DYNAMIC_AABB
const bool BULLET_COMPOUND_USE_DYNAMIC_AABB
Definition: bullet_utils.h:86
collision_detection_bullet::CollisionObjectWrapper::m_touch_links
std::set< std::string > m_touch_links
The robot links the collision objects is allowed to touch.
Definition: bullet_utils.h:205
shapes::Mesh::vertices
double * vertices
collision_detection_bullet::createConvexHull
int createConvexHull(AlignedVector< Eigen::Vector3d > &vertices, std::vector< int > &faces, const AlignedVector< Eigen::Vector3d > &input, double shrink=-1, double shrinkClamp=-1)
Create a convex hull from vertices using Bullet Convex Hull Computer.
Definition: contact_checker_common.h:166
shapes::OcTree
r
S r
collision_detection_bullet::createShapePrimitive
btCollisionShape * createShapePrimitive(const shapes::ShapeConstPtr &geom, const CollisionObjectType &collision_object_type, CollisionObjectWrapper *cow)
Casts a geometric shape into a btCollisionShape.
Definition: bullet_utils.cpp:261
shapes::OCTREE
OCTREE
collision_detection::BodyTypes::Type
Type
The types of bodies that are considered for collision.
Definition: include/moveit/collision_detection/collision_common.h:88
shapes::CYLINDER
CYLINDER
octomap.h
shapes::Cylinder::radius
double radius
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
x
double x
collision_detection_bullet::CollisionObjectType::MULTI_SPHERE
@ MULTI_SPHERE
Use the mesh and represent it by multiple spheres collision object.
shapes::Cylinder::length
double length
shapes::Cone
collision_detection_bullet::AlignedVector
std::vector< T, Eigen::aligned_allocator< T > > AlignedVector
Definition: basic_types.h:53
collision_detection_bullet::CollisionObjectType::CONVEX_HULL
@ CONVEX_HULL
Use the mesh in shapes::Shape but make it a convex hulls collision object (if not convex it will be c...
collision_detection_bullet::CollisionObjectType::USE_SHAPE_TYPE
@ USE_SHAPE_TYPE
Infer the type from the type specified in the shapes::Shape class.
shapes::BOX
BOX
collision_detection_bullet::BULLET_DEFAULT_CONTACT_DISTANCE
const btScalar BULLET_DEFAULT_CONTACT_DISTANCE
Definition: bullet_utils.h:85
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
collision_detection_bullet::BULLET_MARGIN
const btScalar BULLET_MARGIN
Definition: bullet_utils.h:81


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Nov 21 2021 03:25:56