collision_world_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 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 
38 #include <fcl/shape/geometric_shape_to_BVH_model.h>
39 #include <fcl/traversal/traversal_node_bvhs.h>
40 #include <fcl/traversal/traversal_node_setup.h>
41 #include <fcl/collision_node.h>
42 #include <boost/bind.hpp>
43 
45 {
46  auto m = new fcl::DynamicAABBTreeCollisionManager();
47  // m->tree_init_level = 2;
48  manager_.reset(m);
49 
50  // request notifications about changes to new world
51  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
52 }
53 
55 {
56  auto m = new fcl::DynamicAABBTreeCollisionManager();
57  // m->tree_init_level = 2;
58  manager_.reset(m);
59 
60  // request notifications about changes to new world
61  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
62  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
63 }
64 
66  : CollisionWorld(other, world)
67 {
68  auto m = new fcl::DynamicAABBTreeCollisionManager();
69  // m->tree_init_level = 2;
70  manager_.reset(m);
71 
72  fcl_objs_ = other.fcl_objs_;
73  for (auto& fcl_obj : fcl_objs_)
74  fcl_obj.second.registerTo(manager_.get());
75  // manager_->update();
76 
77  // request notifications about changes to new world
78  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
79 }
80 
82 {
83  getWorld()->removeObserver(observer_handle_);
84 }
85 
87  const CollisionRobot& robot,
88  const robot_state::RobotState& state) const
89 {
90  checkRobotCollisionHelper(req, res, robot, state, nullptr);
91 }
92 
94  const CollisionRobot& robot,
95  const robot_state::RobotState& state,
96  const AllowedCollisionMatrix& acm) const
97 {
98  checkRobotCollisionHelper(req, res, robot, state, &acm);
99 }
100 
102  const CollisionRobot& robot,
103  const robot_state::RobotState& state1,
104  const robot_state::RobotState& state2) const
105 {
106  CONSOLE_BRIDGE_logError("FCL continuous collision checking not yet implemented");
107 }
108 
110  const CollisionRobot& robot,
111  const robot_state::RobotState& state1,
112  const robot_state::RobotState& state2,
113  const AllowedCollisionMatrix& acm) const
114 {
115  CONSOLE_BRIDGE_logError("FCL continuous collision checking not yet implemented");
116 }
117 
119  CollisionResult& res,
120  const CollisionRobot& robot,
121  const robot_state::RobotState& state,
122  const AllowedCollisionMatrix* acm) const
123 {
124  const CollisionRobotFCL& robot_fcl = dynamic_cast<const CollisionRobotFCL&>(robot);
125  FCLObject fcl_obj;
126  robot_fcl.constructFCLObject(state, fcl_obj);
127 
128  CollisionData cd(&req, &res, acm);
129  cd.enableGroup(robot.getRobotModel());
130  for (std::size_t i = 0; !cd.done_ && i < fcl_obj.collision_objects_.size(); ++i)
131  manager_->collide(fcl_obj.collision_objects_[i].get(), &cd, &collisionCallback);
132 
133  if (req.distance)
134  res.distance = distanceRobotHelper(robot, state, acm, req.verbose);
135 }
136 
138  const CollisionWorld& other_world) const
139 {
140  checkWorldCollisionHelper(req, res, other_world, nullptr);
141 }
142 
144  const CollisionWorld& other_world,
145  const AllowedCollisionMatrix& acm) const
146 {
147  checkWorldCollisionHelper(req, res, other_world, &acm);
148 }
149 
151  CollisionResult& res,
152  const CollisionWorld& other_world,
153  const AllowedCollisionMatrix* acm) const
154 {
155  const CollisionWorldFCL& other_fcl_world = dynamic_cast<const CollisionWorldFCL&>(other_world);
156  CollisionData cd(&req, &res, acm);
157  manager_->collide(other_fcl_world.manager_.get(), &cd, &collisionCallback);
158 
159  if (req.distance)
160  res.distance = distanceWorldHelper(other_world, acm);
161 }
162 
164 {
165  for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
166  {
167  FCLGeometryConstPtr g = createCollisionGeometry(obj->shapes_[i], obj);
168  if (g)
169  {
170  auto co = new fcl::CollisionObject(g->collision_geometry_, transform2fcl(obj->shape_poses_[i]));
171  fcl_obj.collision_objects_.push_back(FCLCollisionObjectPtr(co));
172  fcl_obj.collision_geometry_.push_back(g);
173  }
174  }
175 }
176 
178 {
179  // remove FCL objects that correspond to this object
180  auto jt = fcl_objs_.find(id);
181  if (jt != fcl_objs_.end())
182  {
183  jt->second.unregisterFrom(manager_.get());
184  jt->second.clear();
185  }
186 
187  // check to see if we have this object
188  auto it = getWorld()->find(id);
189  if (it != getWorld()->end())
190  {
191  // construct FCL objects that correspond to this object
192  if (jt != fcl_objs_.end())
193  {
194  constructFCLObject(it->second.get(), jt->second);
195  jt->second.registerTo(manager_.get());
196  }
197  else
198  {
199  constructFCLObject(it->second.get(), fcl_objs_[id]);
200  fcl_objs_[id].registerTo(manager_.get());
201  }
202  }
203  else
204  {
205  if (jt != fcl_objs_.end())
206  fcl_objs_.erase(jt);
207  }
208 
209  // manager_->update();
210 }
211 
213 {
214  if (world == getWorld())
215  return;
216 
217  // turn off notifications about old world
218  getWorld()->removeObserver(observer_handle_);
219 
220  // clear out objects from old world
221  manager_->clear();
222  fcl_objs_.clear();
224 
226 
227  // request notifications about changes to new world
228  observer_handle_ = getWorld()->addObserver(boost::bind(&CollisionWorldFCL::notifyObjectChange, this, _1, _2));
229 
230  // get notifications any objects already in the new world
231  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
232 }
233 
235 {
236  if (action == World::DESTROY)
237  {
238  auto it = fcl_objs_.find(obj->id_);
239  if (it != fcl_objs_.end())
240  {
241  it->second.unregisterFrom(manager_.get());
242  it->second.clear();
243  fcl_objs_.erase(it);
244  }
246  }
247  else
248  {
249  updateFCLObject(obj->id_);
250  if (action & (World::DESTROY | World::REMOVE_SHAPE))
252  }
253 }
254 
256  const robot_state::RobotState& state,
257  const AllowedCollisionMatrix* acm,
258  bool verbose) const
259 {
260  const CollisionRobotFCL& robot_fcl = dynamic_cast<const CollisionRobotFCL&>(robot);
261  FCLObject fcl_obj;
262  robot_fcl.constructFCLObject(state, fcl_obj);
263 
264  CollisionRequest req;
265  req.verbose = verbose;
266  CollisionResult res;
267  CollisionData cd(&req, &res, acm);
268  cd.enableGroup(robot.getRobotModel());
269 
270  for (std::size_t i = 0; !cd.done_ && i < fcl_obj.collision_objects_.size(); ++i)
271  manager_->distance(fcl_obj.collision_objects_[i].get(), &cd, &distanceCallback);
272 
273  return res.distance;
274 }
275 
277  const robot_state::RobotState& state, bool verbose) const
278 {
279  return distanceRobotHelper(robot, state, nullptr, verbose);
280 }
281 
283  const robot_state::RobotState& state,
284  const AllowedCollisionMatrix& acm, bool verbose) const
285 {
286  return distanceRobotHelper(robot, state, &acm, verbose);
287 }
288 
290 {
291  return distanceWorldHelper(world, nullptr, verbose);
292 }
293 
295  const AllowedCollisionMatrix& acm, bool verbose) const
296 {
297  return distanceWorldHelper(world, &acm, verbose);
298 }
299 
301  const AllowedCollisionMatrix* acm,
302  bool verbose) const
303 {
304  const CollisionWorldFCL& other_fcl_world = dynamic_cast<const CollisionWorldFCL&>(other_world);
305  CollisionRequest req;
306  req.verbose = verbose;
307  CollisionResult res;
308  CollisionData cd(&req, &res, acm);
309  manager_->distance(other_fcl_world.manager_.get(), &cd, &distanceCallback);
310 
311  return res.distance;
312 }
313 
double distanceRobotHelper(const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm, bool verbose=false) const
void updateFCLObject(const std::string &id)
void constructFCLObject(const robot_state::RobotState &state, FCLObject &fcl_obj) const
bool done_
Flag indicating whether collision checking is complete.
void checkRobotCollisionHelper(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm) const
FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr &shape, const robot_model::LinkModel *link, int shape_index)
#define CONSOLE_BRIDGE_logError(fmt,...)
std::vector< shapes::ShapeConstPtr > shapes_
All the shapes making up this object.
Definition: world.h:102
bool collisionCallback(fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data)
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:192
const robot_model::RobotModelConstPtr & getRobotModel() const
The kinematic model corresponding to this collision model.
std::unique_ptr< fcl::BroadPhaseCollisionManager > manager_
virtual double distanceWorld(const CollisionWorld &world, bool verbose=false) const
The shortest distance to another world instance (world)
double distanceWorldHelper(const CollisionWorld &world, const AllowedCollisionMatrix *acm, bool verbose=false) const
World::ObjectConstPtr ObjectConstPtr
virtual void setWorld(const WorldPtr &world)
A representation of an object.
Definition: world.h:80
void checkWorldCollisionHelper(const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world, const AllowedCollisionMatrix *acm) const
bool verbose
Flag indicating whether information about detected collisions should be reported. ...
virtual void checkWorldCollision(const CollisionRequest &req, CollisionResult &res, const CollisionWorld &other_world) const
Check whether a given set of objects is in collision with objects from another world. Any contacts are considered.
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
This class represents a collision model of the robot and can be used for self collision checks (to ch...
virtual void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state) const
Check whether the robot model is in collision with the world. Any collisions between a robot link and...
virtual void setWorld(const WorldPtr &world)
virtual double distanceRobot(const CollisionRobot &robot, const robot_state::RobotState &state, bool verbose=false) const
Compute the shortest distance between a robot and the world.
Perform collision checking with the environment. The collision world maintains a representation of th...
void enableGroup(const robot_model::RobotModelConstPtr &kmodel)
Compute active_components_only_ based on req_.
void transform2fcl(const Eigen::Affine3d &b, fcl::Transform3f &f)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:82
void notifyObjectChange(const ObjectConstPtr &obj, World::Action action)
bool distanceCallback(fcl::CollisionObject *o1, fcl::CollisionObject *o2, void *data, double &min_dist)
EigenSTL::vector_Affine3d shape_poses_
The poses of the corresponding entries in shapes_.
Definition: world.h:107
std::shared_ptr< fcl::CollisionObject > FCLCollisionObjectPtr
std::map< std::string, FCLObject > fcl_objs_
void constructFCLObject(const World::Object *obj, FCLObject &fcl_obj) const


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Jan 21 2018 03:54:28