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 
43 #include <fcl/broadphase/broadphase.h>
44 #include <fcl/collision.h>
45 #include <fcl/distance.h>
46 #include <memory>
47 #include <set>
48 
49 namespace collision_detection
50 {
51 MOVEIT_STRUCT_FORWARD(CollisionGeometryData);
52 
54 {
56  {
57  ptr.link = link;
58  }
59 
61  : type(BodyTypes::ROBOT_ATTACHED), shape_index(index)
62  {
63  ptr.ab = ab;
64  }
65 
66  CollisionGeometryData(const World::Object* obj, int index) : type(BodyTypes::WORLD_OBJECT), shape_index(index)
67  {
68  ptr.obj = obj;
69  }
70 
71  const std::string& getID() const
72  {
73  switch (type)
74  {
76  return ptr.link->getName();
78  return ptr.ab->getName();
79  default:
80  break;
81  }
82  return ptr.obj->id_;
83  }
84 
85  std::string getTypeString() const
86  {
87  switch (type)
88  {
90  return "Robot link";
92  return "Robot attached";
93  default:
94  break;
95  }
96  return "Object";
97  }
98 
100  bool sameObject(const CollisionGeometryData& other) const
101  {
102  return type == other.type && ptr.raw == other.ptr.raw;
103  }
104 
107  union
108  {
112  const void* raw;
113  } ptr;
114 };
115 
117 {
118  CollisionData() : req_(NULL), active_components_only_(NULL), res_(NULL), acm_(NULL), done_(false)
119  {
120  }
121 
123  : req_(req), active_components_only_(NULL), res_(res), acm_(acm), done_(false)
124  {
125  }
126 
128  {
129  }
130 
132  void enableGroup(const robot_model::RobotModelConstPtr& kmodel);
133 
136 
140  const std::set<const robot_model::LinkModel*>* active_components_only_;
141 
144 
147 
149  bool done_;
150 };
151 
153 {
154  DistanceData(const DistanceRequest* req, DistanceResult* res) : req(req), res(res), done(false)
155  {
156  }
158  {
159  }
160 
163 
166 
168  bool done;
169 };
170 
172 
174 {
176  {
177  }
178 
179  FCLGeometry(fcl::CollisionGeometry* collision_geometry, const robot_model::LinkModel* link, int shape_index)
180  : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(link, shape_index))
181  {
182  collision_geometry_->setUserData(collision_geometry_data_.get());
183  }
184 
185  FCLGeometry(fcl::CollisionGeometry* collision_geometry, const robot_state::AttachedBody* ab, int shape_index)
186  : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(ab, shape_index))
187  {
188  collision_geometry_->setUserData(collision_geometry_data_.get());
189  }
190 
191  FCLGeometry(fcl::CollisionGeometry* collision_geometry, const World::Object* obj, int shape_index)
192  : collision_geometry_(collision_geometry), collision_geometry_data_(new CollisionGeometryData(obj, shape_index))
193  {
194  collision_geometry_->setUserData(collision_geometry_data_.get());
195  }
196 
197  template <typename T>
198  void updateCollisionGeometryData(const T* data, int shape_index, bool newType)
199  {
200  if (!newType && collision_geometry_data_)
201  if (collision_geometry_data_->ptr.raw == reinterpret_cast<const void*>(data))
202  return;
203  collision_geometry_data_.reset(new CollisionGeometryData(data, shape_index));
204  collision_geometry_->setUserData(collision_geometry_data_.get());
205  }
206 
207  std::shared_ptr<fcl::CollisionGeometry> collision_geometry_;
208  CollisionGeometryDataPtr collision_geometry_data_;
209 };
210 
211 typedef std::shared_ptr<fcl::CollisionObject> FCLCollisionObjectPtr;
212 typedef std::shared_ptr<const fcl::CollisionObject> FCLCollisionObjectConstPtr;
213 
214 struct FCLObject
215 {
216  void registerTo(fcl::BroadPhaseCollisionManager* manager);
217  void unregisterFrom(fcl::BroadPhaseCollisionManager* manager);
218  void clear();
219 
220  std::vector<FCLCollisionObjectPtr> collision_objects_;
221  std::vector<FCLGeometryConstPtr> collision_geometry_;
222 };
223 
225 {
227  std::shared_ptr<fcl::BroadPhaseCollisionManager> manager_;
228 };
229 
230 bool collisionCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data);
231 
232 bool distanceCallback(fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* data, double& min_dist);
233 
234 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_model::LinkModel* link,
235  int shape_index);
236 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const robot_state::AttachedBody* ab,
237  int shape_index);
238 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, const World::Object* obj);
239 
240 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
242 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
244 FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape, double scale, double padding,
245  const World::Object* obj);
247 
248 inline void transform2fcl(const Eigen::Affine3d& b, fcl::Transform3f& f)
249 {
250  Eigen::Quaterniond q(b.linear());
251  f.setTranslation(fcl::Vec3f(b.translation().x(), b.translation().y(), b.translation().z()));
252  f.setQuatRotation(fcl::Quaternion3f(q.w(), q.x(), q.y(), q.z()));
253 }
254 
255 inline fcl::Transform3f transform2fcl(const Eigen::Affine3d& b)
256 {
257  fcl::Transform3f t;
258  transform2fcl(b, t);
259  return t;
260 }
261 
262 inline void fcl2contact(const fcl::Contact& fc, Contact& c)
263 {
264  c.pos = Eigen::Vector3d(fc.pos[0], fc.pos[1], fc.pos[2]);
265  c.normal = Eigen::Vector3d(fc.normal[0], fc.normal[1], fc.normal[2]);
266  c.depth = fc.penetration_depth;
267  const CollisionGeometryData* cgd1 = static_cast<const CollisionGeometryData*>(fc.o1->getUserData());
268  c.body_name_1 = cgd1->getID();
269  c.body_type_1 = cgd1->type;
270  const CollisionGeometryData* cgd2 = static_cast<const CollisionGeometryData*>(fc.o2->getUserData());
271  c.body_name_2 = cgd2->getID();
272  c.body_type_2 = cgd2->type;
273 }
274 
275 inline void fcl2costsource(const fcl::CostSource& fcs, CostSource& cs)
276 {
277  cs.aabb_min[0] = fcs.aabb_min[0];
278  cs.aabb_min[1] = fcs.aabb_min[1];
279  cs.aabb_min[2] = fcs.aabb_min[2];
280 
281  cs.aabb_max[0] = fcs.aabb_max[0];
282  cs.aabb_max[1] = fcs.aabb_max[1];
283  cs.aabb_max[2] = fcs.aabb_max[2];
284 
285  cs.cost = fcs.cost_density;
286 }
287 }
288 
289 #endif
bool sameObject(const CollisionGeometryData &other) const
Check if two CollisionGeometryData objects point to the same source object.
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.
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const robot_model::LinkModel *link, int shape_index)
BodyType body_type_2
The type of the second body involved in the contact.
union collision_detection::CollisionGeometryData::@0 ptr
void updateCollisionGeometryData(const T *data, int shape_index, bool newType)
bool collisionCallback(fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data)
FCLGeometry(fcl::CollisionGeometry *collision_geometry, const robot_state::AttachedBody *ab, int shape_index)
Generic interface to collision detection.
FCLGeometry(fcl::CollisionGeometry *collision_geometry, const robot_model::LinkModel *link, int shape_index)
std::shared_ptr< fcl::BroadPhaseCollisionManager > manager_
A representation of an object.
Definition: world.h:80
MOVEIT_STRUCT_FORWARD(CollisionGeometryData)
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...
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::CollisionObject > FCLCollisionObjectConstPtr
const AllowedCollisionMatrix * acm_
The user specified collision matrix (may be NULL)
void fcl2costsource(const fcl::CostSource &fcs, CostSource &cs)
std::string body_name_1
The id of the first body involved in the contact.
void transform2fcl(const Eigen::Affine3d &b, fcl::Transform3f &f)
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
FCLGeometry(fcl::CollisionGeometry *collision_geometry, const World::Object *obj, int shape_index)
When collision costs are computed, this structure contains information about the partial cost incurre...
bool distanceCallback(fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data, double &min_dist)
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::CollisionObject > FCLCollisionObjectPtr
const DistanceRequest * req
Distance query request information.
const std::set< const robot_model::LinkModel * > * active_components_only_
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33