collision_world_distance_field.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, 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 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: E. Gil Jones */
36 
37 #include <ros/console.h>
41 #include <boost/bind.hpp>
42 #include <memory>
43 
44 namespace collision_detection
45 {
47 {
48  getWorld()->removeObserver(observer_handle_);
49 }
50 
51 CollisionWorldDistanceField::CollisionWorldDistanceField(Eigen::Vector3d size, Eigen::Vector3d origin,
52  bool use_signed_distance_field, double resolution,
53  double collision_tolerance, double max_propogation_distance)
54  : CollisionWorld()
55  , size_(size)
56  , origin_(origin)
57  , use_signed_distance_field_(use_signed_distance_field)
58  , resolution_(resolution)
59  , collision_tolerance_(collision_tolerance)
60  , max_propogation_distance_(max_propogation_distance)
61 {
63 
64  // request notifications about changes to world
66  getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2));
67 }
68 
69 CollisionWorldDistanceField::CollisionWorldDistanceField(const WorldPtr& world, Eigen::Vector3d size,
70  Eigen::Vector3d origin, bool use_signed_distance_field,
71  double resolution, double collision_tolerance,
72  double max_propogation_distance)
73  : CollisionWorld(world)
74  , size_(size)
75  , origin_(origin)
76  , use_signed_distance_field_(use_signed_distance_field)
77  , resolution_(resolution)
78  , collision_tolerance_(collision_tolerance)
79  , max_propogation_distance_(max_propogation_distance)
80 {
82 
83  // request notifications about changes to world
85  getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2));
86  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
87 }
88 
90  const WorldPtr& world)
91  : CollisionWorld(other, world)
92 {
93  size_ = other.size_;
94  origin_ = other.origin_;
96  resolution_ = other.resolution_;
100 
101  // request notifications about changes to world
103  getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2));
104  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
105 }
106 
108  const CollisionRobot& robot,
109  const robot_state::RobotState& state) const
110 {
111  GroupStateRepresentationPtr gsr;
112  checkCollision(req, res, robot, state, gsr);
113 }
114 
116  const CollisionRobot& robot, const robot_state::RobotState& state,
117  GroupStateRepresentationPtr& gsr) const
118 {
119  try
120  {
121  const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot);
122  if (!gsr)
123  {
124  cdr.generateCollisionCheckingStructures(req.group_name, state, NULL, gsr, true);
125  }
126  else
127  {
128  cdr.updateGroupStateRepresentationState(state, gsr);
129  }
130  bool done = cdr.getSelfCollisions(req, res, gsr);
131  if (!done)
132  {
133  done = cdr.getIntraGroupCollisions(req, res, gsr);
134  }
135  if (!done)
136  {
137  getEnvironmentCollisions(req, res, distance_field_cache_entry_->distance_field_, gsr);
138  }
139  }
140  catch (const std::bad_cast& e)
141  {
142  ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what());
143  return;
144  }
145 
146  (const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr;
147 }
148 
150  const CollisionRobot& robot, const robot_state::RobotState& state,
151  const AllowedCollisionMatrix& acm) const
152 {
153  GroupStateRepresentationPtr gsr;
154  checkCollision(req, res, robot, state, acm, gsr);
155 }
156 
158  const CollisionRobot& robot, const robot_state::RobotState& state,
159  const AllowedCollisionMatrix& acm,
160  GroupStateRepresentationPtr& gsr) const
161 {
162  try
163  {
164  const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot);
165  if (!gsr)
166  {
167  cdr.generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true);
168  }
169  else
170  {
171  cdr.updateGroupStateRepresentationState(state, gsr);
172  }
173  bool done = cdr.getSelfCollisions(req, res, gsr);
174  if (!done)
175  {
176  done = cdr.getIntraGroupCollisions(req, res, gsr);
177  }
178  if (!done)
179  {
180  getEnvironmentCollisions(req, res, distance_field_cache_entry_->distance_field_, gsr);
181  }
182  }
183  catch (const std::bad_cast& e)
184  {
185  ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what());
186  return;
187  }
188 
189  (const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr;
190 }
191 
193  const CollisionRobot& robot,
194  const robot_state::RobotState& state) const
195 {
196  GroupStateRepresentationPtr gsr;
197  checkRobotCollision(req, res, robot, state, gsr);
198 }
199 
201  const CollisionRobot& robot, const robot_state::RobotState& state,
202  GroupStateRepresentationPtr& gsr) const
203 {
204  distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_->distance_field_;
205  try
206  {
207  const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot);
208  DistanceFieldCacheEntryConstPtr dfce;
209  if (!gsr)
210  {
211  cdr.generateCollisionCheckingStructures(req.group_name, state, NULL, gsr, false);
212  }
213  else
214  {
215  cdr.updateGroupStateRepresentationState(state, gsr);
216  }
217  getEnvironmentCollisions(req, res, env_distance_field, gsr);
218  (const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr;
219 
220  // checkRobotCollisionHelper(req, res, robot, state, &acm);
221  }
222  catch (const std::bad_cast& e)
223  {
224  ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what());
225  return;
226  }
227 }
228 
230  const CollisionRobot& robot, const robot_state::RobotState& state,
231  const AllowedCollisionMatrix& acm) const
232 {
233  GroupStateRepresentationPtr gsr;
234  checkRobotCollision(req, res, robot, state, acm, gsr);
235 }
236 
238  const CollisionRobot& robot, const robot_state::RobotState& state,
239  const AllowedCollisionMatrix& acm,
240  GroupStateRepresentationPtr& gsr) const
241 {
242  distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_->distance_field_;
243  try
244  {
245  const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot);
246  DistanceFieldCacheEntryPtr dfce;
247  if (!gsr)
248  {
249  cdr.generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true);
250  }
251  else
252  {
253  cdr.updateGroupStateRepresentationState(state, gsr);
254  }
255  getEnvironmentCollisions(req, res, env_distance_field, gsr);
256  (const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr;
257 
258  // checkRobotCollisionHelper(req, res, robot, state, &acm);
259  }
260  catch (const std::bad_cast& e)
261  {
262  ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what());
263  return;
264  }
265 }
266 
268  const CollisionRobot& robot,
269  const robot_state::RobotState& state,
270  const AllowedCollisionMatrix* acm,
271  GroupStateRepresentationPtr& gsr) const
272 {
273  distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_->distance_field_;
274  try
275  {
276  const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot);
277  if (!gsr)
278  {
279  cdr.generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true);
280  }
281  else
282  {
283  cdr.updateGroupStateRepresentationState(state, gsr);
284  }
285  cdr.getSelfProximityGradients(gsr);
287  getEnvironmentProximityGradients(env_distance_field, gsr);
288  }
289  catch (const std::bad_cast& e)
290  {
291  ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what());
292  return;
293  }
294 
295  (const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr;
296 }
297 
299  const CollisionRobot& robot, const robot_state::RobotState& state,
300  const AllowedCollisionMatrix* acm,
301  GroupStateRepresentationPtr& gsr) const
302 {
303  try
304  {
305  const CollisionRobotDistanceField& cdr = dynamic_cast<const CollisionRobotDistanceField&>(robot);
306  if (!gsr)
307  {
308  cdr.generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true);
309  }
310  else
311  {
312  cdr.updateGroupStateRepresentationState(state, gsr);
313  }
314  cdr.getSelfCollisions(req, res, gsr);
315  cdr.getIntraGroupCollisions(req, res, gsr);
316  distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_->distance_field_;
317  getEnvironmentCollisions(req, res, env_distance_field, gsr);
318  }
319  catch (const std::bad_cast& e)
320  {
321  ROS_ERROR_STREAM("Could not cast CollisionRobot to CollisionRobotDistanceField, " << e.what());
322  return;
323  }
324 
325  (const_cast<CollisionWorldDistanceField*>(this))->last_gsr_ = gsr;
326 }
327 
329  const CollisionRequest& req, CollisionResult& res, const distance_field::DistanceFieldConstPtr& env_distance_field,
330  GroupStateRepresentationPtr& gsr) const
331 {
332  for (unsigned int i = 0; i < gsr->dfce_->link_names_.size() + gsr->dfce_->attached_body_names_.size(); i++)
333  {
334  bool is_link = i < gsr->dfce_->link_names_.size();
335  std::string link_name = i < gsr->dfce_->link_names_.size() ? gsr->dfce_->link_names_[i] : "attached";
336  if (is_link && !gsr->dfce_->link_has_geometry_[i])
337  {
338  continue;
339  }
340 
341  const std::vector<CollisionSphere>* collision_spheres_1;
342  const EigenSTL::vector_Vector3d* sphere_centers_1;
343 
344  if (is_link)
345  {
346  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
347  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
348  }
349  else
350  {
351  collision_spheres_1 =
352  &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
353  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
354  }
355 
356  if (req.contacts)
357  {
358  std::vector<unsigned int> colls;
359  bool coll = getCollisionSphereCollision(
360  env_distance_field.get(), *collision_spheres_1, *sphere_centers_1, max_propogation_distance_,
361  collision_tolerance_, std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls);
362  if (coll)
363  {
364  res.collision = true;
365  for (unsigned int j = 0; j < colls.size(); j++)
366  {
367  Contact con;
368  if (is_link)
369  {
370  con.pos = gsr->link_body_decompositions_[i]->getSphereCenters()[colls[j]];
372  con.body_name_1 = gsr->dfce_->link_names_[i];
373  }
374  else
375  {
376  con.pos =
377  gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()[colls[j]];
379  con.body_name_1 = gsr->dfce_->attached_body_names_[i - gsr->dfce_->link_names_.size()];
380  }
381 
383  con.body_name_2 = "environment";
384  res.contact_count++;
385  res.contacts[std::pair<std::string, std::string>(con.body_name_1, con.body_name_2)].push_back(con);
386  gsr->gradients_[i].types[colls[j]] = ENVIRONMENT;
387  // ROS_DEBUG_STREAM("Link " << dfce->link_names_[i] << " sphere " <<
388  // colls[j] << " in env collision");
389  }
390 
391  gsr->gradients_[i].collision = true;
392  if (res.contact_count >= req.max_contacts)
393  {
394  return true;
395  }
396  }
397  }
398  else
399  {
400  bool coll = getCollisionSphereCollision(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1,
402  if (coll)
403  {
404  res.collision = true;
405  return true;
406  }
407  }
408  }
409  return (res.contact_count >= req.max_contacts);
410 }
411 
413  const distance_field::DistanceFieldConstPtr& env_distance_field, GroupStateRepresentationPtr& gsr) const
414 {
415  bool in_collision = false;
416  for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++)
417  {
418  bool is_link = i < gsr->dfce_->link_names_.size();
419 
420  if (is_link && !gsr->dfce_->link_has_geometry_[i])
421  {
422  continue;
423  }
424 
425  const std::vector<CollisionSphere>* collision_spheres_1;
426  const EigenSTL::vector_Vector3d* sphere_centers_1;
427  if (is_link)
428  {
429  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
430  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
431  }
432  else
433  {
434  collision_spheres_1 =
435  &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
436  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
437  }
438 
439  bool coll = getCollisionSphereGradients(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1,
440  gsr->gradients_[i], ENVIRONMENT, collision_tolerance_, false,
442  if (coll)
443  {
444  in_collision = true;
445  }
446  }
447  return in_collision;
448 }
449 
450 void CollisionWorldDistanceField::setWorld(const WorldPtr& world)
451 {
452  if (world == getWorld())
453  return;
454 
455  // turn off notifications about old world
456  getWorld()->removeObserver(observer_handle_);
457 
458  // clear out objects from old world
459  distance_field_cache_entry_->distance_field_->reset();
460 
462 
463  // request notifications about changes to new world
465  getWorld()->addObserver(boost::bind(&CollisionWorldDistanceField::notifyObjectChange, this, _1, _2));
466 
467  // get notifications any objects already in the new world
468  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
469 }
470 
472  World::Action action)
473 {
475 
476  EigenSTL::vector_Vector3d add_points;
477  EigenSTL::vector_Vector3d subtract_points;
478  self->updateDistanceObject(obj->id_, self->distance_field_cache_entry_, add_points, subtract_points);
479 
480  if (action == World::DESTROY)
481  {
482  self->distance_field_cache_entry_->distance_field_->removePointsFromField(subtract_points);
483  }
484  else if (action & (World::MOVE_SHAPE | World::REMOVE_SHAPE))
485  {
486  self->distance_field_cache_entry_->distance_field_->removePointsFromField(subtract_points);
487  self->distance_field_cache_entry_->distance_field_->addPointsToField(add_points);
488  }
489  else
490  {
491  self->distance_field_cache_entry_->distance_field_->addPointsToField(add_points);
492  }
493 
494  ROS_DEBUG_NAMED("collision_distance_field", "Modifying object %s took %lf s", obj->id_.c_str(),
495  (ros::WallTime::now() - n).toSec());
496 }
497 
498 void CollisionWorldDistanceField::updateDistanceObject(const std::string& id, DistanceFieldCacheEntryPtr& dfce,
499  EigenSTL::vector_Vector3d& add_points,
500  EigenSTL::vector_Vector3d& subtract_points)
501 {
502  std::map<std::string, std::vector<PosedBodyPointDecompositionPtr>>::iterator cur_it =
503  dfce->posed_body_point_decompositions_.find(id);
504  if (cur_it != dfce->posed_body_point_decompositions_.end())
505  {
506  for (unsigned int i = 0; i < cur_it->second.size(); i++)
507  {
508  subtract_points.insert(subtract_points.end(), cur_it->second[i]->getCollisionPoints().begin(),
509  cur_it->second[i]->getCollisionPoints().end());
510  }
511  }
512 
513  World::ObjectConstPtr object = getWorld()->getObject(id);
514  if (object)
515  {
516  ROS_DEBUG_STREAM("Updating/Adding Object '" << object->id_ << "' with " << object->shapes_.size()
517  << " shapes to CollisionWorldDistanceField");
518  std::vector<PosedBodyPointDecompositionPtr> shape_points;
519  for (unsigned int i = 0; i < object->shapes_.size(); i++)
520  {
521  shapes::ShapeConstPtr shape = object->shapes_[i];
522  if (shape->type == shapes::OCTREE)
523  {
524  const shapes::OcTree* octree_shape = static_cast<const shapes::OcTree*>(shape.get());
525  std::shared_ptr<const octomap::OcTree> octree = octree_shape->octree;
526 
527  shape_points.push_back(std::make_shared<PosedBodyPointDecomposition>(octree));
528  }
529  else
530  {
531  BodyDecompositionConstPtr bd = getBodyDecompositionCacheEntry(shape, resolution_);
532 
533  shape_points.push_back(std::make_shared<PosedBodyPointDecomposition>(bd, object->shape_poses_[i]));
534  }
535 
536  add_points.insert(add_points.end(), shape_points.back()->getCollisionPoints().begin(),
537  shape_points.back()->getCollisionPoints().end());
538  }
539 
540  dfce->posed_body_point_decompositions_[id] = shape_points;
541  }
542  else
543  {
544  ROS_DEBUG_STREAM("Removing Object '" << id << "' from CollisionWorldDistanceField");
545  dfce->posed_body_point_decompositions_.erase(id);
546  }
547 }
548 
549 CollisionWorldDistanceField::DistanceFieldCacheEntryPtr CollisionWorldDistanceField::generateDistanceFieldCacheEntry()
550 {
551  DistanceFieldCacheEntryPtr dfce(new DistanceFieldCacheEntry());
552  dfce->distance_field_.reset(new distance_field::PropagationDistanceField(
553  size_.x(), size_.y(), size_.z(), resolution_, origin_.x() - 0.5 * size_.x(), origin_.y() - 0.5 * size_.y(),
555 
556  EigenSTL::vector_Vector3d add_points;
557  EigenSTL::vector_Vector3d subtract_points;
558  for (World::const_iterator it = getWorld()->begin(); it != getWorld()->end(); ++it)
559  {
560  updateDistanceObject(it->first, dfce, add_points, subtract_points);
561  }
562  dfce->distance_field_->addPointsToField(add_points);
563  return dfce;
564 }
565 }
566 
bool getIntraGroupProximityGradients(GroupStateRepresentationPtr &gsr) const
void generateCollisionCheckingStructures(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr, bool generate_distance_field) const
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
bool getEnvironmentProximityGradients(const distance_field::DistanceFieldConstPtr &env_distance_field, GroupStateRepresentationPtr &gsr) const
BodyType body_type_2
The type of the second body involved in the contact.
bool getCollisionSphereGradients(const distance_field::DistanceField *distance_field, const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, GradientInfo &gradient, const CollisionType &type, double tolerance, bool subtract_radii, double maximum_value, bool stop_at_first_collision)
virtual void checkCollision(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state) const
Check whether the robot model is in collision with itself or the world at a particular state...
ContactMap contacts
A map returning the pairs of ids of the bodies in contact, plus information about the contacts themse...
Represents an action that occurred on an object in the world. Several bits may be set indicating seve...
Definition: world.h:195
void updateDistanceObject(const std::string &id, CollisionWorldDistanceField::DistanceFieldCacheEntryPtr &dfce, EigenSTL::vector_Vector3d &add_points, EigenSTL::vector_Vector3d &subtract_points)
void getCollisionGradients(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeConstPtr &shape, double resolution)
CollisionWorldDistanceField(Eigen::Vector3d size=Eigen::Vector3d(DEFAULT_SIZE_X, DEFAULT_SIZE_Y, DEFAULT_SIZE_Z), Eigen::Vector3d origin=Eigen::Vector3d(0, 0, 0), bool use_signed_distance_field=DEFAULT_USE_SIGNED_DISTANCE_FIELD, double resolution=DEFAULT_RESOLUTION, double collision_tolerance=DEFAULT_COLLISION_TOLERANCE, double max_propogation_distance=DEFAULT_MAX_PROPOGATION_DISTANCE)
std::map< std::string, ObjectPtr >::const_iterator const_iterator
Definition: world.h:117
Generic interface to collision detection.
bool collision
True if collision was found, false otherwise.
World::ObjectConstPtr ObjectConstPtr
#define ROS_DEBUG_NAMED(name,...)
std::shared_ptr< const octomap::OcTree > octree
std::size_t max_contacts
Overall maximum number of contacts to compute.
static const double resolution
static void notifyObjectChange(CollisionWorldDistanceField *self, const ObjectConstPtr &obj, World::Action action)
void updateGroupStateRepresentationState(const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
bool getEnvironmentCollisions(const CollisionRequest &req, CollisionResult &res, const distance_field::DistanceFieldConstPtr &env_distance_field, GroupStateRepresentationPtr &gsr) const
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...
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...
This class represents a collision model of the robot and can be used for self collision checks (to ch...
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot) ...
std::string body_name_2
The id of the second body involved in the contact.
bool getIntraGroupCollisions(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const
#define ROS_DEBUG_STREAM(args)
virtual void setWorld(const WorldPtr &world)
std::string body_name_1
The id of the first body involved in the contact.
std::size_t max_contacts_per_pair
Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at differ...
static WallTime now()
Perform collision checking with the environment. The collision world maintains a representation of th...
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
BodyType body_type_1
The type of the first body involved in the contact.
A DistanceField implementation that uses a vector propagation method. Distances propagate outward fro...
#define ROS_ERROR_STREAM(args)
void getAllCollisions(const CollisionRequest &req, CollisionResult &res, const CollisionRobot &robot, const robot_state::RobotState &state, const AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
bool getSelfProximityGradients(GroupStateRepresentationPtr &gsr) const
bool getCollisionSphereCollision(const distance_field::DistanceField *distance_field, const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, double maximum_value, double tolerance)
bool getSelfCollisions(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Sep 15 2019 03:57:56