collision_env_fcl.cpp
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 the copyright holder 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, Jens Petit */
36 
40 
42 
43 #if (MOVEIT_FCL_VERSION >= FCL_VERSION_CHECK(0, 6, 0))
45 #endif
46 
47 namespace collision_detection
48 {
49 static const std::string NAME = "FCL";
50 constexpr char LOGNAME[] = "collision_detection.fcl";
51 
52 namespace
53 {
54 // Check whether this FCL version supports the requested computations
55 void checkFCLCapabilities(const DistanceRequest& req)
56 {
57 #if MOVEIT_FCL_VERSION < FCL_VERSION_CHECK(0, 6, 0)
58  if (req.enable_nearest_points)
59  {
60  // Known issues:
61  // https://github.com/flexible-collision-library/fcl/issues/171,
62  // https://github.com/flexible-collision-library/fcl/pull/288
64  "You requested a distance check with enable_nearest_points=true, "
65  "but the FCL version MoveIt was compiled against (%d.%d.%d) "
66  "is known to return bogus nearest points. Please update your FCL "
67  "to at least 0.6.0.",
68  FCL_MAJOR_VERSION, FCL_MINOR_VERSION, FCL_PATCH_VERSION);
69  }
70 #else
71  (void)(req); // silent -Wunused-parameter
72 #endif
73 }
74 } // namespace
75 
76 CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, double padding, double scale)
77  : CollisionEnv(model, padding, scale)
78 {
79  const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
80  std::size_t index;
81  robot_geoms_.resize(robot_model_->getLinkGeometryCount());
82  robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount());
83  // we keep the same order of objects as what RobotState *::getLinkState() returns
84  for (auto link : links)
85  for (std::size_t j = 0; j < link->getShapes().size(); ++j)
86  {
87  FCLGeometryConstPtr link_geometry = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()),
88  getLinkPadding(link->getName()), link, j);
89  if (link_geometry)
90  {
91  index = link->getFirstCollisionBodyTransformIndex() + j;
92  robot_geoms_[index] = link_geometry;
93 
94  // Need to store the FCL object so the AABB does not get recreated every time.
95  // Every time this object is created, g->computeLocalAABB() is called which is
96  // very expensive and should only be calculated once. To update the AABB, use the
97  // collObj->setTransform and then call collObj->computeAABB() to transform the AABB.
98  robot_fcl_objs_[index] =
99  FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(link_geometry->collision_geometry_));
100  }
101  else
102  ROS_ERROR_NAMED(LOGNAME, "Unable to construct collision geometry for link '%s'", link->getName().c_str());
103  }
104 
105  manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
106 
107  // request notifications about changes to new world
108  observer_handle_ = getWorld()->addObserver(
109  [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
110 }
111 
112 CollisionEnvFCL::CollisionEnvFCL(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding,
113  double scale)
114  : CollisionEnv(model, world, padding, scale)
115 {
116  const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
117  std::size_t index;
118  robot_geoms_.resize(robot_model_->getLinkGeometryCount());
119  robot_fcl_objs_.resize(robot_model_->getLinkGeometryCount());
120  // we keep the same order of objects as what RobotState *::getLinkState() returns
121  for (auto link : links)
122  for (std::size_t j = 0; j < link->getShapes().size(); ++j)
123  {
124  FCLGeometryConstPtr g = createCollisionGeometry(link->getShapes()[j], getLinkScale(link->getName()),
125  getLinkPadding(link->getName()), link, j);
126  if (g)
127  {
128  index = link->getFirstCollisionBodyTransformIndex() + j;
129  robot_geoms_[index] = g;
130 
131  // Need to store the FCL object so the AABB does not get recreated every time.
132  // Every time this object is created, g->computeLocalAABB() is called which is
133  // very expensive and should only be calculated once. To update the AABB, use the
134  // collObj->setTransform and then call collObj->computeAABB() to transform the AABB.
136  }
137  else
138  ROS_ERROR_NAMED(LOGNAME, "Unable to construct collision geometry for link '%s'", link->getName().c_str());
139  }
140 
141  manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
142 
143  // request notifications about changes to new world
144  observer_handle_ = getWorld()->addObserver(
145  [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
146  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
147 }
148 
150 {
151  getWorld()->removeObserver(observer_handle_);
152 }
153 
154 CollisionEnvFCL::CollisionEnvFCL(const CollisionEnvFCL& other, const WorldPtr& world) : CollisionEnv(other, world)
155 {
156  robot_geoms_ = other.robot_geoms_;
157  robot_fcl_objs_ = other.robot_fcl_objs_;
158 
159  manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
160 
161  fcl_objs_ = other.fcl_objs_;
162  for (auto& fcl_obj : fcl_objs_)
163  fcl_obj.second.registerTo(manager_.get());
164  // manager_->update();
165 
166  // request notifications about changes to new world
167  observer_handle_ = getWorld()->addObserver(
168  [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
169 }
170 
172  std::vector<FCLGeometryConstPtr>& geoms) const
173 {
174  const std::vector<shapes::ShapeConstPtr>& shapes = ab->getShapes();
175  const size_t num_shapes = shapes.size();
176  geoms.reserve(num_shapes);
177  for (std::size_t i = 0; i < num_shapes; ++i)
178  {
179  FCLGeometryConstPtr co = createCollisionGeometry(shapes[i], getLinkScale(ab->getAttachedLinkName()),
180  getLinkPadding(ab->getAttachedLinkName()), ab, i);
181  if (co)
182  geoms.push_back(co);
183  }
184 }
185 
187 {
188  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
189  {
190  FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj);
191  if (g)
192  {
193  auto co = new fcl::CollisionObjectd(g->collision_geometry_, transform2fcl(obj->global_shape_poses_[i]));
194  fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(co));
195  fcl_obj.collision_geometry_.push_back(g);
196  }
197  }
198 }
199 
201 {
202  fcl_obj.collision_objects_.reserve(robot_geoms_.size());
204 
205  for (std::size_t i = 0; i < robot_geoms_.size(); ++i)
206  if (robot_geoms_[i] && robot_geoms_[i]->collision_geometry_)
207  {
208  transform2fcl(state.getCollisionBodyTransform(robot_geoms_[i]->collision_geometry_data_->ptr.link,
209  robot_geoms_[i]->collision_geometry_data_->shape_index),
210  fcl_tf);
211  auto coll_obj = new fcl::CollisionObjectd(*robot_fcl_objs_[i]);
212  coll_obj->setTransform(fcl_tf);
213  coll_obj->computeAABB();
214  fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(coll_obj));
215  }
216 
217  // TODO: Implement a method for caching fcl::CollisionObject's for moveit::core::AttachedBody's
218  std::vector<const moveit::core::AttachedBody*> ab;
219  state.getAttachedBodies(ab);
220  for (auto& body : ab)
221  {
222  std::vector<FCLGeometryConstPtr> objs;
223  getAttachedBodyObjects(body, objs);
224  const EigenSTL::vector_Isometry3d& ab_t = body->getGlobalCollisionBodyTransforms();
225  for (std::size_t k = 0; k < objs.size(); ++k)
226  if (objs[k]->collision_geometry_)
227  {
228  transform2fcl(ab_t[k], fcl_tf);
229  fcl_obj.collision_objects_.push_back(
230  std::make_shared<fcl::CollisionObjectd>(objs[k]->collision_geometry_, fcl_tf));
231  // we copy the shared ptr to the CollisionGeometryData, as this is not stored by the class itself,
232  // and would be destroyed when objs goes out of scope.
233  fcl_obj.collision_geometry_.push_back(objs[k]);
234  }
235  }
236 }
237 
238 void CollisionEnvFCL::allocSelfCollisionBroadPhase(const moveit::core::RobotState& state, FCLManager& manager) const
239 {
240  manager.manager_ = std::make_unique<fcl::DynamicAABBTreeCollisionManagerd>();
241 
242  constructFCLObjectRobot(state, manager.object_);
243  manager.object_.registerTo(manager.manager_.get());
244 }
245 
246 void CollisionEnvFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
247  const moveit::core::RobotState& state) const
248 {
249  checkSelfCollisionHelper(req, res, state, nullptr);
250 }
251 
252 void CollisionEnvFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
253  const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const
254 {
255  checkSelfCollisionHelper(req, res, state, &acm);
256 }
257 
258 void CollisionEnvFCL::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res,
259  const moveit::core::RobotState& state,
260  const AllowedCollisionMatrix* acm) const
261 {
262  FCLManager manager;
263  allocSelfCollisionBroadPhase(state, manager);
264  CollisionData cd(&req, &res, acm);
265  cd.enableGroup(getRobotModel());
266  manager.manager_->collide(&cd, &collisionCallback);
267  if (req.distance)
268  {
269  DistanceRequest dreq;
271 
272  dreq.group_name = req.group_name;
273  dreq.acm = acm;
274  dreq.enableGroup(getRobotModel());
275  distanceSelf(dreq, dres, state);
276  res.distance = dres.minimum_distance.distance;
277  }
278 }
279 
281  const moveit::core::RobotState& state) const
282 {
283  checkRobotCollisionHelper(req, res, state, nullptr);
284 }
285 
287  const moveit::core::RobotState& state,
288  const AllowedCollisionMatrix& acm) const
289 {
290  checkRobotCollisionHelper(req, res, state, &acm);
291 }
292 
294  const moveit::core::RobotState& /*state1*/,
295  const moveit::core::RobotState& /*state2*/) const
296 {
297  ROS_ERROR_NAMED(LOGNAME, "Continuous collision not implemented");
298 }
299 
300 void CollisionEnvFCL::checkRobotCollision(const CollisionRequest& /*req*/, CollisionResult& /*res*/,
301  const moveit::core::RobotState& /*state1*/,
302  const moveit::core::RobotState& /*state2*/,
303  const AllowedCollisionMatrix& /*acm*/) const
304 {
305  ROS_ERROR_NAMED(LOGNAME, "Not implemented");
306 }
307 
308 void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, CollisionResult& res,
309  const moveit::core::RobotState& state,
310  const AllowedCollisionMatrix* acm) const
311 {
312  FCLObject fcl_obj;
313  constructFCLObjectRobot(state, fcl_obj);
314 
315  CollisionData cd(&req, &res, acm);
316  cd.enableGroup(getRobotModel());
317  for (std::size_t i = 0; !cd.done_ && i < fcl_obj.collision_objects_.size(); ++i)
318  manager_->collide(fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback);
319 
320  if (req.distance)
321  {
322  DistanceRequest dreq;
323  DistanceResult dres;
324 
325  dreq.group_name = req.group_name;
326  dreq.acm = acm;
327  dreq.enableGroup(getRobotModel());
328  distanceRobot(dreq, dres, state);
329  res.distance = dres.minimum_distance.distance;
330  }
331 }
332 
334  const moveit::core::RobotState& state) const
335 {
336  checkFCLCapabilities(req);
337 
338  FCLManager manager;
339  allocSelfCollisionBroadPhase(state, manager);
340  DistanceData drd(&req, &res);
341 
342  manager.manager_->distance(&drd, &distanceCallback);
343 }
344 
346  const moveit::core::RobotState& state) const
347 {
348  checkFCLCapabilities(req);
349 
350  FCLObject fcl_obj;
351  constructFCLObjectRobot(state, fcl_obj);
352 
353  DistanceData drd(&req, &res);
354  for (std::size_t i = 0; !drd.done && i < fcl_obj.collision_objects_.size(); ++i)
355  manager_->distance(fcl_obj.collision_objects_[i].get(), &drd, &distanceCallback);
356 }
357 
358 void CollisionEnvFCL::updateFCLObject(const std::string& id)
359 {
360  // remove FCL objects that correspond to this object
361  auto jt = fcl_objs_.find(id);
362  if (jt != fcl_objs_.end())
363  {
364  jt->second.unregisterFrom(manager_.get());
365  jt->second.clear();
366  }
367 
368  // check to see if we have this object
369  auto it = getWorld()->find(id);
370  if (it != getWorld()->end())
371  {
372  // construct FCL objects that correspond to this object
373  if (jt != fcl_objs_.end())
374  {
375  constructFCLObjectWorld(it->second.get(), jt->second);
376  jt->second.registerTo(manager_.get());
377  }
378  else
379  {
380  constructFCLObjectWorld(it->second.get(), fcl_objs_[id]);
381  fcl_objs_[id].registerTo(manager_.get());
382  }
383  }
384  else
385  {
386  if (jt != fcl_objs_.end())
387  fcl_objs_.erase(jt);
388  }
389 
390  // manager_->update();
391 }
392 
393 void CollisionEnvFCL::setWorld(const WorldPtr& world)
394 {
395  if (world == getWorld())
396  return;
397 
398  // turn off notifications about old world
399  getWorld()->removeObserver(observer_handle_);
400 
401  // clear out objects from old world
402  manager_->clear();
403  fcl_objs_.clear();
405 
406  CollisionEnv::setWorld(world);
407 
408  // request notifications about changes to new world
409  observer_handle_ = getWorld()->addObserver(
410  [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
411 
412  // get notifications any objects already in the new world
413  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
414 }
415 
416 void CollisionEnvFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Action action)
417 {
418  if (action == World::DESTROY)
419  {
420  auto it = fcl_objs_.find(obj->id_);
421  if (it != fcl_objs_.end())
422  {
423  it->second.unregisterFrom(manager_.get());
424  it->second.clear();
425  fcl_objs_.erase(it);
426  }
428  }
429  else
430  {
431  updateFCLObject(obj->id_);
432  if (action & (World::DESTROY | World::REMOVE_SHAPE))
434  }
435 }
436 
437 void CollisionEnvFCL::updatedPaddingOrScaling(const std::vector<std::string>& links)
438 {
439  std::size_t index;
440  for (const auto& link : links)
441  {
442  const moveit::core::LinkModel* lmodel = robot_model_->getLinkModel(link);
443  if (lmodel)
444  {
445  for (std::size_t j = 0; j < lmodel->getShapes().size(); ++j)
446  {
447  FCLGeometryConstPtr g = createCollisionGeometry(lmodel->getShapes()[j], getLinkScale(lmodel->getName()),
448  getLinkPadding(lmodel->getName()), lmodel, j);
449  if (g)
450  {
452  robot_geoms_[index] = g;
454  }
455  }
456  }
457  else
458  ROS_ERROR_NAMED(LOGNAME, "Updating padding or scaling for unknown link: '%s'", link.c_str());
459  }
460 }
461 
462 const std::string& CollisionDetectorAllocatorFCL::getName() const
463 {
464  return NAME;
465 }
466 
467 } // namespace collision_detection
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
collision_detection::CollisionEnvFCL::checkSelfCollisionHelper
void checkSelfCollisionHelper(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm) const
Bundles the different checkSelfCollision functions into a single function.
Definition: collision_env_fcl.cpp:290
collision_detection::CollisionEnvDistanceField::observer_handle_
World::ObserverHandle observer_handle_
Definition: collision_env_distance_field.h:337
broadphase_dynamic_AABB_tree.h
collision_detection::FCLCollisionObjectConstPtr
std::shared_ptr< const fcl::CollisionObjectd > FCLCollisionObjectConstPtr
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:280
ROS_ERROR_THROTTLE_NAMED
#define ROS_ERROR_THROTTLE_NAMED(period, name,...)
collision_detection::CollisionEnv::getRobotModel
const moveit::core::RobotModelConstPtr & getRobotModel() const
The kinematic model corresponding to this collision model.
Definition: collision_env.h:287
collision_detection::World::DESTROY
@ DESTROY
Definition: world.h:256
collision_detection::CollisionEnvFCL::fcl_objs_
std::map< std::string, FCLObject > fcl_objs_
Definition: collision_env_fcl.h:215
shapes
collision_common.h
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
collision_detection::CollisionEnvFCL::checkRobotCollisionHelper
void checkRobotCollisionHelper(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm) const
Bundles the different checkRobotCollision functions into a single function.
Definition: collision_env_fcl.cpp:340
collision_detection::CollisionEnvFCL::updateFCLObject
void updateFCLObject(const std::string &id)
Updates the specified object in \m fcl_objs_ and in the manager from new data available in the World.
Definition: collision_env_fcl.cpp:390
collision_detection::CollisionEnvFCL::allocSelfCollisionBroadPhase
void allocSelfCollisionBroadPhase(const moveit::core::RobotState &state, FCLManager &manager) const
Prepares for the collision check through constructing an FCL collision object out of the current robo...
Definition: collision_env_fcl.cpp:270
collision_detection::CollisionEnvFCL::manager_
std::unique_ptr< fcl::BroadPhaseCollisionManagerd > manager_
FCL collision manager which handles the collision checking process.
Definition: collision_env_fcl.h:213
collision_detection::CollisionEnvFCL::distanceRobot
void distanceRobot(const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const override
Compute the distance between a robot and the world.
Definition: collision_env_fcl.cpp:377
collision_detection::FCLManager::manager_
std::shared_ptr< fcl::BroadPhaseCollisionManagerd > manager_
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:300
collision_detector_allocator_fcl.h
fcl::Transform3d
Transform3< double > Transform3d
collision_detection::World::Action
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:265
collision_detection::collisionCallback
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 ...
Definition: fcl/src/collision_common.cpp:88
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
collision_detection::CollisionEnvFCL::robot_fcl_objs_
std::vector< FCLCollisionObjectConstPtr > robot_fcl_objs_
Vector of shared pointers to the FCL collision objects which make up the robot.
Definition: collision_env_fcl.h:210
collision_detection::LOGNAME
constexpr char LOGNAME[]
Definition: collision_env_fcl.cpp:82
obj
CollisionObject< S > * obj
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:216
collision_detection::FCLObject::collision_objects_
std::vector< FCLCollisionObjectPtr > collision_objects_
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:290
collision_detection::cleanCollisionGeometryCache
void cleanCollisionGeometryCache()
Increases the counter of the caches which can trigger the cleaning of expired entries from them.
Definition: fcl/src/collision_common.cpp:958
moveit::core::AttachedBody
Object defining bodies that can be attached to robot links.
Definition: attached_body.h:121
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
collision_detection::NAME
static const std::string NAME
Definition: collision_env_fcl.cpp:81
collision_detection::transform2fcl
void transform2fcl(const Eigen::Isometry3d &b, fcl::Transform3d &f)
Transforms an Eigen Isometry3d to FCL coordinate transformation.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:350
collision_detection::distanceCallback
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...
Definition: fcl/src/collision_common.cpp:443
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:177
collision_detection::CollisionEnvFCL::constructFCLObjectWorld
void constructFCLObjectWorld(const World::Object *obj, FCLObject &fcl_obj) const
Construct an FCL collision object from MoveIt's World::Object.
Definition: collision_env_fcl.cpp:218
collision_detection::FCLCollisionObjectPtr
std::shared_ptr< fcl::CollisionObjectd > FCLCollisionObjectPtr
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:279
collision_detection::DistanceRequest::acm
const AllowedCollisionMatrix * acm
The allowed collision matrix used to filter checks.
Definition: include/moveit/collision_detection/collision_common.h:319
collision_detection::DistanceRequest::group_name
std::string group_name
The group name.
Definition: include/moveit/collision_detection/collision_common.h:313
moveit::core::RobotState::getAttachedBodies
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
Definition: robot_state.cpp:1083
moveit::core::LinkModel::getFirstCollisionBodyTransformIndex
int getFirstCollisionBodyTransformIndex() const
Definition: link_model.h:96
collision_detection::World::Object
A representation of an object.
Definition: world.h:79
collision_detection::FCLManager
Bundles an FCLObject and a broadphase FCL collision manager.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:297
collision_detection::CollisionEnv::getLinkScale
const std::map< std::string, double > & getLinkScale() const
Get the link scaling as a map (from link names to scale value)
Definition: collision_env.cpp:219
collision_detection::DistanceResult::minimum_distance
DistanceResultsData minimum_distance
ResultsData for the two objects with the minimum distance.
Definition: include/moveit/collision_detection/collision_common.h:400
collision_detection::createCollisionGeometry
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const moveit::core::LinkModel *link, int shape_index)
Create new FCLGeometry object out of robot link model.
Definition: fcl/src/collision_common.cpp:906
collision_detection::CollisionEnvFCL::setWorld
void setWorld(const WorldPtr &world) override
Definition: collision_env_fcl.cpp:425
collision_detection::CollisionEnvFCL::distanceSelf
void distanceSelf(const DistanceRequest &req, DistanceResult &res, const moveit::core::RobotState &state) const override
The distance to self-collision given the robot is at state state.
Definition: collision_env_fcl.cpp:365
collision_detection::CollisionEnvDistanceField::notifyObjectChange
void notifyObjectChange(const ObjectConstPtr &obj, World::Action action)
Definition: collision_env_distance_field.cpp:1750
collision_env_fcl.h
collision_detection::DistanceResult
Result of a distance request.
Definition: include/moveit/collision_detection/collision_common.h:390
moveit::core::AttachedBody::getAttachedLinkName
const std::string & getAttachedLinkName() const
Get the name of the link this body is attached to.
Definition: attached_body.h:156
collision_detection::CollisionEnvFCL::robot_geoms_
std::vector< FCLGeometryConstPtr > robot_geoms_
Vector of shared pointers to the FCL geometry for the objects in fcl_objs_.
Definition: collision_env_fcl.h:207
moveit::core::RobotState::getCollisionBodyTransform
const Eigen::Isometry3d & getCollisionBodyTransform(const std::string &link_name, std::size_t index)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
Definition: robot_state.h:1492
fcl_compat.h
collision_detection::CollisionEnvFCL::observer_handle_
World::ObserverHandle observer_handle_
Definition: collision_env_fcl.h:221
collision_detection::CollisionEnv::getLinkPadding
const std::map< std::string, double > & getLinkPadding() const
Get the link paddings as a map (from link names to padding value)
Definition: collision_env.cpp:179
collision_detection::CollisionDetectorAllocatorFCL::getName
const std::string & getName() const override
Definition: collision_env_fcl.cpp:494
moveit::core::AttachedBody::getShapes
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
Definition: attached_body.h:168
collision_detection::CollisionEnvFCL::CollisionEnvFCL
CollisionEnvFCL()=delete
collision_detection::FCLObject
A general high-level object which consists of multiple FCLCollisionObjects. It is the top level data ...
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:284
collision_detection::CollisionEnvFCL::checkRobotCollision
void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override
Check whether the robot model is in collision with the world. Any collisions between a robot link and...
Definition: collision_env_fcl.cpp:312
index
unsigned int index
collision_detection::CollisionEnvFCL::constructFCLObjectRobot
void constructFCLObjectRobot(const moveit::core::RobotState &state, FCLObject &fcl_obj) const
Out of the current robot state and its attached bodies construct an FCLObject which can then be used ...
Definition: collision_env_fcl.cpp:232
collision_detection::DistanceData
Data structure which is passed to the distance callback function of the collision manager.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:211
collision_detection::DistanceResultsData::distance
double distance
The distance between two objects. If two objects are in collision, distance <= 0.
Definition: include/moveit/collision_detection/collision_common.h:342
collision_detection::FCLObject::collision_geometry_
std::vector< FCLGeometryConstPtr > collision_geometry_
Geometry data corresponding to collision_objects_.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:293
collision_detection::CollisionEnv::setWorld
virtual void setWorld(const WorldPtr &world)
Definition: collision_env.cpp:282
moveit::core::LinkModel::getShapes
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get shape associated to the collision geometry for this link.
Definition: link_model.h:175
collision_detection::CollisionEnvFCL::~CollisionEnvFCL
~CollisionEnvFCL() override
Definition: collision_env_fcl.cpp:181
moveit::core::LinkModel::getName
const std::string & getName() const
The name of this link.
Definition: link_model.h:80
collision_detection::World::CREATE
@ CREATE
Definition: world.h:255
collision_detection::CollisionData
Data structure which is passed to the collision callback function of the collision manager.
Definition: fcl/include/moveit/collision_detection_fcl/collision_common.h:173
collision_detection::DistanceRequest
Representation of a distance-reporting request.
Definition: include/moveit/collision_detection/collision_common.h:274
collision_detection::CollisionEnvFCL::getAttachedBodyObjects
void getAttachedBodyObjects(const moveit::core::AttachedBody *ab, std::vector< FCLGeometryConstPtr > &geoms) const
Converts all shapes which make up an atttached body into a vector of FCLGeometryConstPtr.
Definition: collision_env_fcl.cpp:203
collision_detection::DistanceRequest::enableGroup
void enableGroup(const moveit::core::RobotModelConstPtr &robot_model)
Compute active_components_only_ based on req_.
Definition: include/moveit/collision_detection/collision_common.h:290
collision_detection::CollisionEnvFCL::notifyObjectChange
void notifyObjectChange(const ObjectConstPtr &obj, World::Action action)
Callback function executed for each change to the world environment.
Definition: collision_env_fcl.cpp:448
collision_detection::CollisionEnv::robot_model_
moveit::core::RobotModelConstPtr robot_model_
The kinematic model corresponding to this collision model.
Definition: collision_env.h:347
fcl::CollisionObject
collision_detection::CollisionEnvFCL::updatedPaddingOrScaling
void updatedPaddingOrScaling(const std::vector< std::string > &links) override
Updates the FCL collision geometry and objects saved in the CollisionRobotFCL members to reflect a ne...
Definition: collision_env_fcl.cpp:469
collision_detection::World::REMOVE_SHAPE
@ REMOVE_SHAPE
Definition: world.h:259
collision_detection::CollisionEnv::getWorld
const WorldPtr & getWorld()
Definition: collision_env.h:272
fcl::CollisionObjectd
CollisionObject< double > CollisionObjectd
collision_detection::CollisionEnvFCL::checkSelfCollision
void checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override
Check for robot self collision. Any collision between any pair of links is checked for,...
Definition: collision_env_fcl.cpp:278
collision_detection
Definition: collision_detector_allocator_allvalid.h:42


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Sep 18 2022 02:26:15