fcl/include/moveit/collision_detection_fcl/collision_common.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
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  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef MOVEIT_COLLISION_DETECTION_FCL_COLLISION_COMMON_
38 #define MOVEIT_COLLISION_DETECTION_FCL_COLLISION_COMMON_
39 
44 
45 #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
46 #include <fcl/broadphase/broadphase_collision_manager.h>
47 #include <fcl/narrowphase/collision.h>
48 #include <fcl/narrowphase/distance.h>
49 #else
50 #include <fcl/broadphase/broadphase.h>
51 #include <fcl/collision.h>
52 #include <fcl/distance.h>
53 #endif
54 
55 #include <memory>
56 #include <set>
57 
58 namespace collision_detection
59 {
60 MOVEIT_STRUCT_FORWARD(CollisionGeometryData);
61 
64 {
67  {
68  ptr.link = link;
69  }
70 
73  : type(BodyTypes::ROBOT_ATTACHED), shape_index(index)
74  {
75  ptr.ab = ab;
76  }
77 
79  CollisionGeometryData(const World::Object* obj, int index) : type(BodyTypes::WORLD_OBJECT), shape_index(index)
80  {
81  ptr.obj = obj;
82  }
83 
85  const std::string& getID() const
86  {
87  switch (type)
88  {
90  return ptr.link->getName();
92  return ptr.ab->getName();
93  default:
94  break;
95  }
96  return ptr.obj->id_;
97  }
98 
100  std::string getTypeString() const
101  {
102  switch (type)
103  {
105  return "Robot link";
107  return "Robot attached";
108  default:
109  break;
110  }
111  return "Object";
112  }
113 
115  bool sameObject(const CollisionGeometryData& other) const
116  {
117  return type == other.type && ptr.raw == other.ptr.raw;
118  }
119 
122 
128 
130  union
131  {
135  const void* raw;
136  } ptr;
137 };
138 
141 {
142  CollisionData() : req_(NULL), active_components_only_(NULL), res_(NULL), acm_(NULL), done_(false)
143  {
144  }
145 
147  : req_(req), active_components_only_(NULL), res_(res), acm_(acm), done_(false)
148  {
149  }
150 
152  {
153  }
154 
156  void enableGroup(const robot_model::RobotModelConstPtr& robot_model);
157 
160 
165  const std::set<const robot_model::LinkModel*>* active_components_only_;
166 
169 
172 
174  bool done_;
175 };
176 
179 {
180  DistanceData(const DistanceRequest* req, DistanceResult* res) : req(req), res(res), done(false)
181  {
182  }
184  {
185  }
186 
189 
192 
194  bool done;
195 };
196 
198 
201 {
203  {
204  }
205 
208  : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(link, shape_index))
209  {
210  collision_geometry_->setUserData(collision_geometry_data_.get());
211  }
212 
215  : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(ab, shape_index))
216  {
217  collision_geometry_->setUserData(collision_geometry_data_.get());
218  }
219 
222  : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(obj, shape_index))
223  {
224  collision_geometry_->setUserData(collision_geometry_data_.get());
225  }
226 
229  template <typename T>
230  void updateCollisionGeometryData(const T* data, int shape_index, bool newType)
231  {
232  if (!newType && collision_geometry_data_)
233  if (collision_geometry_data_->ptr.raw == reinterpret_cast<const void*>(data))
234  return;
235  collision_geometry_data_.reset(new CollisionGeometryData(data, shape_index));
236  collision_geometry_->setUserData(collision_geometry_data_.get());
237  }
238 
240  std::shared_ptr<fcl::CollisionGeometryd> collision_geometry_;
241 
243  CollisionGeometryDataPtr collision_geometry_data_;
244 };
245 
246 typedef std::shared_ptr<fcl::CollisionObjectd> FCLCollisionObjectPtr;
247 typedef std::shared_ptr<const fcl::CollisionObjectd> FCLCollisionObjectConstPtr;
248 
251 struct FCLObject
252 {
253  void registerTo(fcl::BroadPhaseCollisionManagerd* manager);
254  void unregisterFrom(fcl::BroadPhaseCollisionManagerd* manager);
255  void clear();
256 
257  std::vector<FCLCollisionObjectPtr> collision_objects_;
258 
260  std::vector<FCLGeometryConstPtr> collision_geometry_;
261 };
262 
265 {
267  std::shared_ptr<fcl::BroadPhaseCollisionManagerd> manager_;
268 };
269 
278 
286 bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data, double& min_dist);
287 
289 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link,
290  int shape_index);
291 
293 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_state::AttachedBody* ab,
294  int shape_index);
295 
299 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj);
300 
302 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
304 
306 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
308 
310 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
311  const World::Object* obj);
312 
315 
317 inline void transform2fcl(const Eigen::Isometry3d& b, fcl::Transform3d& f)
318 {
319 #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
320  f = b.matrix();
321 #else
322  Eigen::Quaterniond q(b.rotation());
323  f.setTranslation(fcl::Vector3d(b.translation().x(), b.translation().y(), b.translation().z()));
324  f.setQuatRotation(fcl::Quaternion3f(q.w(), q.x(), q.y(), q.z()));
325 #endif
326 }
327 
329 inline fcl::Transform3d transform2fcl(const Eigen::Isometry3d& b)
330 {
332  transform2fcl(b, t);
333  return t;
334 }
335 
337 inline void fcl2contact(const fcl::Contactd& fc, Contact& c)
338 {
339  c.pos = Eigen::Vector3d(fc.pos[0], fc.pos[1], fc.pos[2]);
340  c.normal = Eigen::Vector3d(fc.normal[0], fc.normal[1], fc.normal[2]);
341  c.depth = fc.penetration_depth;
342  const CollisionGeometryData* cgd1 = static_cast<const CollisionGeometryData*>(fc.o1->getUserData());
343  c.body_name_1 = cgd1->getID();
344  c.body_type_1 = cgd1->type;
345  const CollisionGeometryData* cgd2 = static_cast<const CollisionGeometryData*>(fc.o2->getUserData());
346  c.body_name_2 = cgd2->getID();
347  c.body_type_2 = cgd2->type;
348 }
349 
351 inline void fcl2costsource(const fcl::CostSourced& fcs, CostSource& cs)
352 {
353  cs.aabb_min[0] = fcs.aabb_min[0];
354  cs.aabb_min[1] = fcs.aabb_min[1];
355  cs.aabb_min[2] = fcs.aabb_min[2];
356 
357  cs.aabb_max[0] = fcs.aabb_max[0];
358  cs.aabb_max[1] = fcs.aabb_max[1];
359  cs.aabb_max[2] = fcs.aabb_max[2];
360 
361  cs.cost = fcs.cost_density;
362 }
363 }
364 
365 #endif
Wrapper around world, link and attached objects&#39; geometry data.
CollisionGeometryData(const World::Object *obj, int index)
Constructor for a new world collision geometry.
fcl::CollisionObject CollisionObjectd
Definition: fcl_compat.h:51
Core components of MoveIt!
const CollisionRequest * req_
The collision request passed by the user.
CollisionResult * res_
The user-specified response location.
double cost
The partial cost (the probability of existance for the object there is a collision with) ...
Type
The types of bodies that are considered for collision.
bool done_
Flag indicating whether collision checking is complete.
boost::array< double, 3 > aabb_min
The minimum bound of the AABB defining the volume responsible for this partial cost.
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:90
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const robot_model::LinkModel *link, int shape_index)
Create new FCLGeometry object out of robot link model.
CollisionGeometryData(const robot_model::LinkModel *link, int index)
Constructor for a robot link collision geometry object.
BodyType body_type_2
The type of the second body involved in the contact.
union collision_detection::CollisionGeometryData::@0 ptr
Points to the type of body which contains the geometry.
void updateCollisionGeometryData(const T *data, int shape_index, bool newType)
Updates the collision_geometry_data_ with new data while also setting the collision_geometry_ to the ...
fcl::Transform3f Transform3d
Definition: fcl_compat.h:55
void fcl2contact(const fcl::Contactd &fc, Contact &c)
Transforms an FCL contact into a MoveIt contact point.
std::shared_ptr< fcl::CollisionObjectd > FCLCollisionObjectPtr
Bundles an FCLObject and a broadphase FCL collision manager.
bool sameObject(const CollisionGeometryData &other) const
Check if two CollisionGeometryData objects point to the same source object.
geometry_msgs::TransformStamped t
Generic interface to collision detection.
Data structure which is passed to the collision callback function of the collision manager...
std::shared_ptr< fcl::BroadPhaseCollisionManagerd > manager_
Data structure which is passed to the distance callback function of the collision manager...
FCLGeometry(fcl::CollisionGeometryd *collision_geometry, const robot_model::LinkModel *link, int shape_index)
Constructor for a robot link.
std::string getTypeString() const
Returns a string of the corresponding type.
CollisionGeometryDataPtr collision_geometry_data_
Pointer to the user-defined geometry data.
void transform2fcl(const Eigen::Isometry3d &b, fcl::Transform3d &f)
Transforms an Eigen Isometry3d to FCL coordinate transformation.
A representation of an object.
Definition: world.h:80
MOVEIT_STRUCT_FORWARD(CollisionGeometryData)
fcl::CollisionGeometry CollisionGeometryd
Definition: fcl_compat.h:49
CollisionData(const CollisionRequest *req, CollisionResult *res, const AllowedCollisionMatrix *acm)
boost::array< double, 3 > aabb_max
The maximum bound of the AABB defining the volume responsible for this partial cost.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d pos
contact position
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
void cleanCollisionGeometryCache()
Increases the counter of the caches which can trigger the cleaning of expired entries from them...
Bundles the CollisionGeometryData and FCL collision geometry representation into a single class...
std::vector< FCLGeometryConstPtr > collision_geometry_
Geometry data corresponding to collision_objects_.
std::string body_name_2
The id of the second body involved in the contact.
Eigen::Vector3d normal
normal unit vector at contact
std::shared_ptr< const fcl::CollisionObjectd > FCLCollisionObjectConstPtr
CollisionGeometryData(const robot_state::AttachedBody *ab, int index)
Constructor for a new collision geometry object which is attached to the robot.
fcl::Contact Contactd
Definition: fcl_compat.h:57
bool distanceCallback(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, void *data, double &min_dist)
Callback function used by the FCLManager used for each pair of collision objects to calculate collisi...
bool collisionCallback(fcl::CollisionObjectd *o1, fcl::CollisionObjectd *o2, void *data)
Callback function used by the FCLManager used for each pair of collision objects to calculate object ...
const AllowedCollisionMatrix * acm_
The user-specified collision matrix (may be NULL).
std::string body_name_1
The id of the first body involved in the contact.
FCLGeometry(fcl::CollisionGeometryd *collision_geometry, const World::Object *obj, int shape_index)
Constructor for a world object.
BodyType body_type_1
The type of the first body involved in the contact.
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
int shape_index
Multiple CollisionGeometryData objects construct a collision object. The collision object refers to a...
const std::string & getID() const
Returns the name which is saved in the member pointed to in ptr.
When collision costs are computed, this structure contains information about the partial cost incurre...
A general high-level object which consists of multiple FCLCollisionObjects. It is the top level data ...
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
std::shared_ptr< fcl::CollisionGeometryd > collision_geometry_
Pointer to FCL collision geometry.
fcl::CostSource CostSourced
Definition: fcl_compat.h:59
const DistanceRequest * req
Distance query request information.
FCLGeometry(fcl::CollisionGeometryd *collision_geometry, const robot_state::AttachedBody *ab, int shape_index)
Constructor for an attached body.
fcl::BroadPhaseCollisionManager BroadPhaseCollisionManagerd
Definition: fcl_compat.h:53
void fcl2costsource(const fcl::CostSourced &fcs, CostSource &cs)
Transforms the FCL internal representation to the MoveIt CostSource data structure.
const std::set< const robot_model::LinkModel * > * active_components_only_
If the collision request includes a group name, this set contains the pointers to the link models tha...
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Nov 20 2019 03:55:07