collision_env_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 
38 #include <ros/console.h>
39 #include <ros/assert.h>
40 #include <tf2_eigen/tf2_eigen.h>
45 #include <functional>
46 #include <memory>
47 #include <utility>
48 
49 namespace collision_detection
50 {
51 namespace
52 {
53 static const std::string NAME = "DISTANCE_FIELD";
54 const double EPSILON = 0.001f;
55 } // namespace
56 
58  const moveit::core::RobotModelConstPtr& robot_model,
59  const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions, double size_x, double size_y,
60  double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution,
61  double collision_tolerance, double max_propogation_distance, double /*padding*/, double /*scale*/)
62  : CollisionEnv(robot_model)
63 {
64  initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field,
65  resolution, collision_tolerance, max_propogation_distance);
66 
67  setPadding(0.0);
68 
69  distance_field_cache_entry_world_ = generateDistanceFieldCacheEntryWorld();
70 
71  // request notifications about changes to world
72  observer_handle_ = getWorld()->addObserver(
73  std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
74 }
75 
77  const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world,
78  const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions, double size_x, double size_y,
79  double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution,
80  double collision_tolerance, double max_propogation_distance, double padding, double scale)
81  : CollisionEnv(robot_model, world, padding, scale)
82 {
83  initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field,
84  resolution, collision_tolerance, max_propogation_distance);
85 
86  distance_field_cache_entry_world_ = generateDistanceFieldCacheEntryWorld();
87 
88  // request notifications about changes to world
89  observer_handle_ = getWorld()->addObserver(
90  std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
91 
92  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
93 }
94 
96  : CollisionEnv(other, world)
97 {
98  size_ = other.size_;
99  origin_ = other.origin_;
100 
102  resolution_ = other.resolution_;
110  planning_scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
111 
112  // request notifications about changes to world
113  observer_handle_ = getWorld()->addObserver(
114  std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
115  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
116 }
117 
119 {
120  getWorld()->removeObserver(observer_handle_);
121 }
122 
124  const std::map<std::string, std::vector<CollisionSphere>>& link_body_decompositions, const Eigen::Vector3d& size,
125  const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance,
126  double max_propogation_distance)
127 {
128  size_ = size;
129  origin_ = origin;
130  use_signed_distance_field_ = use_signed_distance_field;
131  resolution_ = resolution;
132  collision_tolerance_ = collision_tolerance;
133  max_propogation_distance_ = max_propogation_distance;
134  addLinkBodyDecompositions(resolution_, link_body_decompositions);
136  planning_scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
137 
138  const std::vector<const moveit::core::JointModelGroup*>& jmg = robot_model_->getJointModelGroups();
139  for (const moveit::core::JointModelGroup* jm : jmg)
140  {
141  std::map<std::string, bool> updated_group_entry;
142  std::vector<std::string> links = jm->getUpdatedLinkModelsWithGeometryNames();
143  for (const std::string& link : links)
144  {
145  updated_group_entry[link] = true;
146  }
147  in_group_update_map_[jm->getName()] = updated_group_entry;
148  state.updateLinkTransforms();
149  DistanceFieldCacheEntryPtr dfce =
150  generateDistanceFieldCacheEntry(jm->getName(), state, &planning_scene_->getAllowedCollisionMatrix(), false);
152  }
153 }
154 
156  const std::string& group_name, const moveit::core::RobotState& state,
157  const collision_detection::AllowedCollisionMatrix* acm, GroupStateRepresentationPtr& gsr,
158  bool generate_distance_field) const
159 {
160  DistanceFieldCacheEntryConstPtr dfce = getDistanceFieldCacheEntry(group_name, state, acm);
161  if (!dfce || (generate_distance_field && !dfce->distance_field_))
162  {
163  // ROS_DEBUG_STREAM_NAMED("collision_distance_field", "Generating new
164  // DistanceFieldCacheEntry for CollisionRobot");
165  DistanceFieldCacheEntryPtr new_dfce =
166  generateDistanceFieldCacheEntry(group_name, state, acm, generate_distance_field);
167  boost::mutex::scoped_lock slock(update_cache_lock_);
168  (const_cast<CollisionEnvDistanceField*>(this))->distance_field_cache_entry_ = new_dfce;
169  dfce = new_dfce;
170  }
171  getGroupStateRepresentation(dfce, state, gsr);
172 }
173 
176  const moveit::core::RobotState& state,
178  GroupStateRepresentationPtr& gsr) const
179 {
180  if (!gsr)
181  {
182  generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true);
183  }
184  else
185  {
187  }
188  // ros::WallTime n = ros::WallTime::now();
189  bool done = getSelfCollisions(req, res, gsr);
190 
191  if (!done)
192  {
193  getIntraGroupCollisions(req, res, gsr);
194  ROS_DEBUG_COND(res.collision, "Intra Group Collision found");
195  }
196 }
197 
198 DistanceFieldCacheEntryConstPtr
199 CollisionEnvDistanceField::getDistanceFieldCacheEntry(const std::string& group_name,
200  const moveit::core::RobotState& state,
202 {
203  DistanceFieldCacheEntryConstPtr ret;
205  {
206  ROS_DEBUG_STREAM("No current Distance field cache entry");
207  return ret;
208  }
209  DistanceFieldCacheEntryConstPtr cur = distance_field_cache_entry_;
210  if (group_name != cur->group_name_)
211  {
212  ROS_DEBUG("No cache entry as group name changed from %s to %s", cur->group_name_.c_str(), group_name.c_str());
213  return ret;
214  }
215  else if (!compareCacheEntryToState(cur, state))
216  {
217  // Regenerating distance field as state has changed from last time
218  // ROS_DEBUG_STREAM_NAMED("collision_distance_field", "Regenerating distance field as
219  // state has changed from last time");
220  return ret;
221  }
222  else if (acm && !compareCacheEntryToAllowedCollisionMatrix(cur, *acm))
223  {
224  ROS_DEBUG("Regenerating distance field as some relevant part of the acm changed");
225  return ret;
226  }
227  return cur;
228 }
229 
232  const moveit::core::RobotState& state) const
233 {
234  GroupStateRepresentationPtr gsr;
235  checkSelfCollisionHelper(req, res, state, nullptr, gsr);
236 }
237 
240  const moveit::core::RobotState& state,
241  GroupStateRepresentationPtr& gsr) const
242 {
243  checkSelfCollisionHelper(req, res, state, nullptr, gsr);
244 }
245 
248  const moveit::core::RobotState& state,
250 {
251  GroupStateRepresentationPtr gsr;
252  checkSelfCollisionHelper(req, res, state, &acm, gsr);
253 }
254 
257  const moveit::core::RobotState& state,
259  GroupStateRepresentationPtr& gsr) const
260 {
261  if (gsr)
262  {
263  ROS_WARN("Shouldn't be calling this function with initialized gsr - ACM "
264  "will be ignored");
265  }
266  checkSelfCollisionHelper(req, res, state, &acm, gsr);
267 }
268 
271  GroupStateRepresentationPtr& gsr) const
272 {
273  for (unsigned int i = 0; i < gsr->dfce_->link_names_.size() + gsr->dfce_->attached_body_names_.size(); i++)
274  {
275  bool is_link = i < gsr->dfce_->link_names_.size();
276  if ((is_link && !gsr->dfce_->link_has_geometry_[i]) || !gsr->dfce_->self_collision_enabled_[i])
277  continue;
278  const std::vector<CollisionSphere>* collision_spheres_1;
279  const EigenSTL::vector_Vector3d* sphere_centers_1;
280 
281  if (is_link)
282  {
283  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
284  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
285  }
286  else
287  {
288  collision_spheres_1 =
289  &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
290  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
291  }
292 
293  if (req.contacts)
294  {
295  std::vector<unsigned int> colls;
296  bool coll =
297  getCollisionSphereCollision(gsr->dfce_->distance_field_.get(), *collision_spheres_1, *sphere_centers_1,
299  std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls);
300  if (coll)
301  {
302  res.collision = true;
303  for (unsigned int col : colls)
304  {
306  if (is_link)
307  {
308  con.pos = gsr->link_body_decompositions_[i]->getSphereCenters()[col];
310  con.body_name_1 = gsr->dfce_->link_names_[i];
311  }
312  else
313  {
314  con.pos = gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()[col];
316  con.body_name_1 = gsr->dfce_->attached_body_names_[i - gsr->dfce_->link_names_.size()];
317  }
318 
319  ROS_DEBUG_STREAM("Self collision detected for link " << con.body_name_1);
320 
322  con.body_name_2 = "self";
323  res.contact_count++;
324  res.contacts[std::pair<std::string, std::string>(con.body_name_1, con.body_name_2)].push_back(con);
325  gsr->gradients_[i].types[col] = SELF;
326  }
327  gsr->gradients_[i].collision = true;
328 
329  if (res.contact_count >= req.max_contacts)
330  {
331  return true;
332  }
333  }
334  }
335  else
336  {
337  bool coll = getCollisionSphereCollision(gsr->dfce_->distance_field_.get(), *collision_spheres_1,
338  *sphere_centers_1, max_propogation_distance_, collision_tolerance_);
339  if (coll)
340  {
341  ROS_DEBUG("Link %s in self collision", gsr->dfce_->link_names_[i].c_str());
342  res.collision = true;
343  return true;
344  }
345  }
346  }
347  return (res.contact_count >= req.max_contacts);
348 }
349 
350 bool CollisionEnvDistanceField::getSelfProximityGradients(GroupStateRepresentationPtr& gsr) const
351 {
352  bool in_collision = false;
353 
354  for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++)
355  {
356  const std::string& link_name = gsr->dfce_->link_names_[i];
357  bool is_link = i < gsr->dfce_->link_names_.size();
358  if ((is_link && !gsr->dfce_->link_has_geometry_[i]) || !gsr->dfce_->self_collision_enabled_[i])
359  {
360  continue;
361  }
362 
363  const std::vector<CollisionSphere>* collision_spheres_1;
364  const EigenSTL::vector_Vector3d* sphere_centers_1;
365  if (is_link)
366  {
367  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
368  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
369  }
370  else
371  {
372  collision_spheres_1 =
373  &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
374  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
375  }
376 
377  // computing distance gradients by checking collisions against other mobile
378  // links as indicated by the AllowedCollisionMatrix
379  bool coll = false;
380  if (gsr->dfce_->acm_.getSize() > 0)
381  {
383  for (unsigned int j = 0; j < gsr->dfce_->link_names_.size(); j++)
384  {
385  // on self collisions skip
386  if (link_name == gsr->dfce_->link_names_[j])
387  {
388  continue;
389  }
390 
391  // on collision exceptions skip
392  if (gsr->dfce_->acm_.getEntry(link_name, gsr->dfce_->link_names_[j], col_type) &&
393  col_type != AllowedCollision::NEVER)
394  {
395  continue;
396  }
397 
398  if (gsr->link_distance_fields_[j])
399  {
400  coll = gsr->link_distance_fields_[j]->getCollisionSphereGradients(
401  *collision_spheres_1, *sphere_centers_1, gsr->gradients_[i], collision_detection::SELF,
403 
404  if (coll)
405  {
406  in_collision = true;
407  }
408  }
409  }
410  }
411 
412  coll = getCollisionSphereGradients(gsr->dfce_->distance_field_.get(), *collision_spheres_1, *sphere_centers_1,
413  gsr->gradients_[i], collision_detection::SELF, collision_tolerance_, false,
415 
416  if (coll)
417  {
418  in_collision = true;
419  }
420  }
421 
422  return in_collision;
423 }
424 
427  GroupStateRepresentationPtr& gsr) const
428 {
429  unsigned int num_links = gsr->dfce_->link_names_.size();
430  unsigned int num_attached_bodies = gsr->dfce_->attached_body_names_.size();
431 
432  for (unsigned int i = 0; i < num_links + num_attached_bodies; i++)
433  {
434  for (unsigned int j = i + 1; j < num_links + num_attached_bodies; j++)
435  {
436  if (i == j)
437  continue;
438  bool i_is_link = i < num_links;
439  bool j_is_link = j < num_links;
440 
441  if ((i_is_link && !gsr->dfce_->link_has_geometry_[i]) || (j_is_link && !gsr->dfce_->link_has_geometry_[j]))
442  continue;
443 
444  if (!gsr->dfce_->intra_group_collision_enabled_[i][j])
445  continue;
446 
447  if (i_is_link && j_is_link &&
448  !doBoundingSpheresIntersect(gsr->link_body_decompositions_[i], gsr->link_body_decompositions_[j]))
449  {
450  // ROS_DEBUG_STREAM("Bounding spheres for " <<
451  // gsr->dfce_->link_names_[i] << " and " << gsr->dfce_->link_names_[j]
452  //<< " don't intersect");
453  continue;
454  }
455  else if (!i_is_link || !j_is_link)
456  {
457  bool all_ok = true;
458  if (!i_is_link && j_is_link)
459  {
460  for (unsigned int k = 0; k < gsr->attached_body_decompositions_[i - num_links]->getSize(); k++)
461  {
463  gsr->link_body_decompositions_[j],
464  gsr->attached_body_decompositions_[i - num_links]->getPosedBodySphereDecomposition(k)))
465  {
466  all_ok = false;
467  break;
468  }
469  }
470  }
471  else if (i_is_link && !j_is_link)
472  {
473  for (unsigned int k = 0; k < gsr->attached_body_decompositions_[j - num_links]->getSize(); k++)
474  {
476  gsr->link_body_decompositions_[i],
477  gsr->attached_body_decompositions_[j - num_links]->getPosedBodySphereDecomposition(k)))
478  {
479  all_ok = false;
480  break;
481  }
482  }
483  }
484  else
485  {
486  for (unsigned int k = 0; k < gsr->attached_body_decompositions_[i - num_links]->getSize() && all_ok; k++)
487  {
488  for (unsigned int l = 0; l < gsr->attached_body_decompositions_[j - num_links]->getSize(); l++)
489  {
491  gsr->attached_body_decompositions_[i - num_links]->getPosedBodySphereDecomposition(k),
492  gsr->attached_body_decompositions_[j - num_links]->getPosedBodySphereDecomposition(l)))
493  {
494  all_ok = false;
495  break;
496  }
497  }
498  }
499  }
500  if (all_ok)
501  {
502  continue;
503  }
504  // std::cerr << "Bounding spheres for " << gsr->dfce_->link_names_[i] <<
505  // " and " << gsr->dfce_->link_names_[j]
506  // << " intersect" << std::endl;
507  }
508  int num_pair = -1;
509  std::string name_1;
510  std::string name_2;
511  if (i_is_link)
512  {
513  name_1 = gsr->dfce_->link_names_[i];
514  }
515  else
516  {
517  name_1 = gsr->dfce_->attached_body_names_[i - num_links];
518  }
519 
520  if (j_is_link)
521  {
522  name_2 = gsr->dfce_->link_names_[j];
523  }
524  else
525  {
526  name_2 = gsr->dfce_->attached_body_names_[j - num_links];
527  }
528  if (req.contacts)
529  {
530  collision_detection::CollisionResult::ContactMap::iterator it =
531  res.contacts.find(std::pair<std::string, std::string>(name_1, name_2));
532  if (it == res.contacts.end())
533  {
534  num_pair = 0;
535  }
536  else
537  {
538  num_pair = it->second.size();
539  }
540  }
541  const std::vector<CollisionSphere>* collision_spheres_1;
542  const std::vector<CollisionSphere>* collision_spheres_2;
543  const EigenSTL::vector_Vector3d* sphere_centers_1;
544  const EigenSTL::vector_Vector3d* sphere_centers_2;
545  if (i_is_link)
546  {
547  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
548  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
549  }
550  else
551  {
552  collision_spheres_1 = &(gsr->attached_body_decompositions_[i - num_links]->getCollisionSpheres());
553  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - num_links]->getSphereCenters());
554  }
555  if (j_is_link)
556  {
557  collision_spheres_2 = &(gsr->link_body_decompositions_[j]->getCollisionSpheres());
558  sphere_centers_2 = &(gsr->link_body_decompositions_[j]->getSphereCenters());
559  }
560  else
561  {
562  collision_spheres_2 = &(gsr->attached_body_decompositions_[j - num_links]->getCollisionSpheres());
563  sphere_centers_2 = &(gsr->attached_body_decompositions_[j - num_links]->getSphereCenters());
564  }
565 
566  for (unsigned int k = 0; k < collision_spheres_1->size() && num_pair < (int)req.max_contacts_per_pair; k++)
567  {
568  for (unsigned int l = 0; l < collision_spheres_2->size() && num_pair < (int)req.max_contacts_per_pair; l++)
569  {
570  Eigen::Vector3d gradient = (*sphere_centers_1)[k] - (*sphere_centers_2)[l];
571  double dist = gradient.norm();
572  // std::cerr << "Dist is " << dist << " rad " <<
573  // (*collision_spheres_1)[k].radius_+(*collision_spheres_2)[l].radius_
574  // << std::endl;
575 
576  if (dist < (*collision_spheres_1)[k].radius_ + (*collision_spheres_2)[l].radius_)
577  {
578  // ROS_DEBUG("Intra-group contact between %s and %s, d =
579  // %f < r1 = %f + r2 = %f", name_1.c_str(),
580  // name_2.c_str(),
581  // dist ,(*collision_spheres_1)[k].radius_
582  // ,(*collision_spheres_2)[l].radius_);
583  // Eigen::Vector3d sc1 = (*sphere_centers_1)[k];
584  // Eigen::Vector3d sc2 = (*sphere_centers_2)[l];
585  // ROS_DEBUG("sphere center 1:[ %f, %f, %f ], sphere
586  // center 2: [%f, %f,%f ], lbdc size =
587  // %i",sc1[0],sc1[1],sc1[2],
588  // sc2[0],sc2[1],sc2[2],int(gsr->link_body_decompositions_.size()));
589  res.collision = true;
590 
591  if (req.contacts)
592  {
594  con.pos = gsr->link_body_decompositions_[i]->getSphereCenters()[k];
595  con.body_name_1 = name_1;
596  con.body_name_2 = name_2;
597  if (i_is_link)
598  {
600  }
601  else
602  {
604  }
605  if (j_is_link)
606  {
608  }
609  else
610  {
612  }
613  res.contact_count++;
614  res.contacts[std::pair<std::string, std::string>(con.body_name_1, con.body_name_2)].push_back(con);
615  num_pair++;
616  // std::cerr << "Pushing back intra " << con.body_name_1 << " and
617  // " << con.body_name_2 << std::endl;
618  gsr->gradients_[i].types[k] = INTRA;
619  gsr->gradients_[i].collision = true;
620  gsr->gradients_[j].types[l] = INTRA;
621  gsr->gradients_[j].collision = true;
622  // ROS_INFO_STREAM("Sphere 1 " << (*sphere_centers_1)[k]);
623  // ROS_INFO_STREAM("Sphere 2 " << (*sphere_centers_2)[l]);
624  // ROS_INFO_STREAM("Norm " << gradient.norm());
625  // ROS_INFO_STREAM("Dist is " << dist
626  // << " radius 1 " <<
627  // (*collision_spheres_1)[k].radius_
628  // << " radius 2 " <<
629  // (*collision_spheres_2)[l].radius_);
630  // ROS_INFO_STREAM("Gradient " << gradient);
631  // ROS_INFO_STREAM("Spheres intersect for " <<
632  // gsr->dfce_->link_names_[i] << " and " <<
633  // gsr->dfce_->link_names_[j]);
634  // std::cerr << "Spheres intersect for " <<
635  // gsr->dfce_->link_names_[i] << " and " <<
636  // gsr->dfce_->link_names_[j] << std::cerr;
637  if (res.contact_count >= req.max_contacts)
638  {
639  return true;
640  }
641  }
642  else
643  {
644  return true;
645  }
646  }
647  }
648  }
649  }
650  }
651  return false;
652 }
653 
654 bool CollisionEnvDistanceField::getIntraGroupProximityGradients(GroupStateRepresentationPtr& gsr) const
655 {
656  bool in_collision = false;
657  unsigned int num_links = gsr->dfce_->link_names_.size();
658  unsigned int num_attached_bodies = gsr->dfce_->attached_body_names_.size();
659  // TODO - deal with attached bodies
660  for (unsigned int i = 0; i < num_links + num_attached_bodies; i++)
661  {
662  for (unsigned int j = i + 1; j < num_links + num_attached_bodies; j++)
663  {
664  if (i == j)
665  continue;
666  bool i_is_link = i < num_links;
667  bool j_is_link = j < num_links;
668  if ((i_is_link && !gsr->dfce_->link_has_geometry_[i]) || (j_is_link && !gsr->dfce_->link_has_geometry_[j]))
669  continue;
670  if (!gsr->dfce_->intra_group_collision_enabled_[i][j])
671  continue;
672  const std::vector<CollisionSphere>* collision_spheres_1;
673  const std::vector<CollisionSphere>* collision_spheres_2;
674  const EigenSTL::vector_Vector3d* sphere_centers_1;
675  const EigenSTL::vector_Vector3d* sphere_centers_2;
676  if (i_is_link)
677  {
678  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
679  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
680  }
681  else
682  {
683  collision_spheres_1 = &(gsr->attached_body_decompositions_[i - num_links]->getCollisionSpheres());
684  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - num_links]->getSphereCenters());
685  }
686  if (j_is_link)
687  {
688  collision_spheres_2 = &(gsr->link_body_decompositions_[j]->getCollisionSpheres());
689  sphere_centers_2 = &(gsr->link_body_decompositions_[j]->getSphereCenters());
690  }
691  else
692  {
693  collision_spheres_2 = &(gsr->attached_body_decompositions_[j - num_links]->getCollisionSpheres());
694  sphere_centers_2 = &(gsr->attached_body_decompositions_[j - num_links]->getSphereCenters());
695  }
696  for (unsigned int k = 0; k < collision_spheres_1->size(); k++)
697  {
698  for (unsigned int l = 0; l < collision_spheres_2->size(); l++)
699  {
700  Eigen::Vector3d gradient = (*sphere_centers_1)[k] - (*sphere_centers_2)[l];
701  double dist = gradient.norm();
702  if (dist < gsr->gradients_[i].distances[k])
703  {
704  gsr->gradients_[i].distances[k] = dist;
705  gsr->gradients_[i].gradients[k] = gradient;
706  gsr->gradients_[i].types[k] = INTRA;
707  }
708  if (dist < gsr->gradients_[j].distances[l])
709  {
710  gsr->gradients_[j].distances[l] = dist;
711  gsr->gradients_[j].gradients[l] = -gradient;
712  gsr->gradients_[j].types[l] = INTRA;
713  }
714  }
715  }
716  }
717  }
718  return in_collision;
719 }
721  const std::string& group_name, const moveit::core::RobotState& state,
722  const collision_detection::AllowedCollisionMatrix* acm, bool generate_distance_field) const
723 {
724  DistanceFieldCacheEntryPtr dfce(new DistanceFieldCacheEntry());
725 
726  if (robot_model_->getJointModelGroup(group_name) == nullptr)
727  {
728  ROS_WARN("No group %s", group_name.c_str());
729  return dfce;
730  }
731 
732  dfce->group_name_ = group_name;
733  dfce->state_ = std::make_shared<moveit::core::RobotState>(state);
734  if (acm)
735  {
736  dfce->acm_ = *acm;
737  }
738  else
739  {
740  ROS_WARN_STREAM("Allowed Collision Matrix is null, enabling all collisions "
741  "in the DistanceFieldCacheEntry");
742  }
743 
744  // generateAllowedCollisionInformation(dfce);
745  dfce->link_names_ = robot_model_->getJointModelGroup(group_name)->getUpdatedLinkModelNames();
746  std::vector<const moveit::core::AttachedBody*> all_attached_bodies;
747  dfce->state_->getAttachedBodies(all_attached_bodies);
748  unsigned int att_count = 0;
749 
750  // may be bigger than necessary
751  std::vector<bool> all_true(dfce->link_names_.size() + all_attached_bodies.size(), true);
752  std::vector<bool> all_false(dfce->link_names_.size() + all_attached_bodies.size(), false);
753 
754  const std::vector<const moveit::core::LinkModel*>& lsv = state.getJointModelGroup(group_name)->getUpdatedLinkModels();
755  dfce->self_collision_enabled_.resize(dfce->link_names_.size() + all_attached_bodies.size(), true);
756  dfce->intra_group_collision_enabled_.resize(dfce->link_names_.size() + all_attached_bodies.size());
757 
758  for (unsigned int i = 0; i < dfce->link_names_.size(); i++)
759  {
760  std::string link_name = dfce->link_names_[i];
761  const moveit::core::LinkModel* link_state = dfce->state_->getLinkModel(link_name);
762  bool found = false;
763 
764  for (unsigned int j = 0; j < lsv.size(); j++)
765  {
766  if (lsv[j]->getName() == link_name)
767  {
768  dfce->link_state_indices_.push_back(j);
769  found = true;
770  break;
771  }
772  }
773 
774  if (!found)
775  {
776  ROS_DEBUG("No link state found for link %s", dfce->link_names_[i].c_str());
777  continue;
778  }
779 
780  if (!link_state->getShapes().empty())
781  {
782  dfce->link_has_geometry_.push_back(true);
783  dfce->link_body_indices_.push_back(link_body_decomposition_index_map_.find(link_name)->second);
784 
785  if (acm)
786  {
788  if (acm->getEntry(link_name, link_name, t) && t == collision_detection::AllowedCollision::ALWAYS)
789  {
790  dfce->self_collision_enabled_[i] = false;
791  }
792 
793  dfce->intra_group_collision_enabled_[i] = all_true;
794  for (unsigned int j = i + 1; j < dfce->link_names_.size(); j++)
795  {
796  if (link_name == dfce->link_names_[j])
797  {
798  dfce->intra_group_collision_enabled_[i][j] = false;
799  continue;
800  }
801  if (acm->getEntry(link_name, dfce->link_names_[j], t) && t == collision_detection::AllowedCollision::ALWAYS)
802  {
803  dfce->intra_group_collision_enabled_[i][j] = false;
804  }
805  }
806 
807  std::vector<const moveit::core::AttachedBody*> link_attached_bodies;
808  state.getAttachedBodies(link_attached_bodies, link_state);
809  for (unsigned int j = 0; j < link_attached_bodies.size(); j++, att_count++)
810  {
811  dfce->attached_body_names_.push_back(link_attached_bodies[j]->getName());
812  dfce->attached_body_link_state_indices_.push_back(dfce->link_state_indices_[i]);
813 
814  if (acm->getEntry(link_name, link_attached_bodies[j]->getName(), t))
815  {
817  {
818  dfce->intra_group_collision_enabled_[i][att_count + dfce->link_names_.size()] = false;
819  }
820  }
821  // std::cerr << "Checking touch links for " << link_name << " and " <<
822  // attached_bodies[j]->getName()
823  // << " num " << attached_bodies[j]->getTouchLinks().size()
824  // << std::endl;
825  // touch links take priority
826  if (link_attached_bodies[j]->getTouchLinks().find(link_name) != link_attached_bodies[j]->getTouchLinks().end())
827  {
828  dfce->intra_group_collision_enabled_[i][att_count + dfce->link_names_.size()] = false;
829  // std::cerr << "Setting intra group for " << link_name << " and
830  // attached body " << link_attached_bodies[j]->getName() << " to
831  // false" << std::endl;
832  }
833  }
834  }
835  else
836  {
837  dfce->self_collision_enabled_[i] = true;
838  dfce->intra_group_collision_enabled_[i] = all_true;
839  }
840  }
841  else
842  {
843  dfce->link_has_geometry_.push_back(false);
844  dfce->link_body_indices_.push_back(0);
845  dfce->self_collision_enabled_[i] = false;
846  dfce->intra_group_collision_enabled_[i] = all_false;
847  }
848  }
849 
850  for (unsigned int i = 0; i < dfce->attached_body_names_.size(); i++)
851  {
852  dfce->intra_group_collision_enabled_[i + dfce->link_names_.size()] = all_true;
853  if (acm)
854  {
856  if (acm->getEntry(dfce->attached_body_names_[i], dfce->attached_body_names_[i], t) &&
858  {
859  dfce->self_collision_enabled_[i + dfce->link_names_.size()] = false;
860  }
861  for (unsigned int j = i + 1; j < dfce->attached_body_names_.size(); j++)
862  {
863  if (acm->getEntry(dfce->attached_body_names_[i], dfce->attached_body_names_[j], t) &&
865  {
866  dfce->intra_group_collision_enabled_[i + dfce->link_names_.size()][j + dfce->link_names_.size()] = false;
867  }
868  // TODO - allow for touch links to be attached bodies?
869  // else {
870  // std::cerr << "Setting not allowed for " << link_name << " and " <<
871  // dfce->link_names_[j] << std::endl;
872  //}
873  }
874  }
875  }
876 
877  std::map<std::string, GroupStateRepresentationPtr>::const_iterator it =
878  pregenerated_group_state_representation_map_.find(dfce->group_name_);
880  {
881  dfce->pregenerated_group_state_representation_ = it->second;
882  }
883 
884  std::map<std::string, bool> updated_map;
885  if (!dfce->link_names_.empty())
886  {
887  const std::vector<const moveit::core::JointModel*>& child_joint_models =
888  dfce->state_->getJointModelGroup(dfce->group_name_)->getActiveJointModels();
889  for (const moveit::core::JointModel* child_joint_model : child_joint_models)
890  {
891  updated_map[child_joint_model->getName()] = true;
892  ROS_DEBUG_STREAM("Joint " << child_joint_model->getName() << " has been added to updated list");
893  }
894  }
895 
896  const std::vector<std::string>& state_variable_names = state.getVariableNames();
897  for (const std::string& state_variable_name : state_variable_names)
898  {
899  double val = state.getVariablePosition(state_variable_name);
900  dfce->state_values_.push_back(val);
901  if (updated_map.count(state_variable_name) == 0)
902  {
903  dfce->state_check_indices_.push_back(dfce->state_values_.size() - 1);
904  ROS_DEBUG_STREAM("Non-group joint " << state_variable_name << " will be checked for state changes");
905  }
906  }
907 
908  if (generate_distance_field)
909  {
910  if (dfce->distance_field_)
911  {
912  ROS_DEBUG_STREAM("CollisionRobot skipping distance field generation, "
913  "will use existing one");
914  }
915  else
916  {
917  std::vector<PosedBodyPointDecompositionPtr> non_group_link_decompositions;
918  std::vector<PosedBodyPointDecompositionVectorPtr> non_group_attached_body_decompositions;
919  const std::map<std::string, bool>& updated_group_map = in_group_update_map_.find(group_name)->second;
920  for (const moveit::core::LinkModel* link_model : robot_model_->getLinkModelsWithCollisionGeometry())
921  {
922  const std::string& link_name = link_model->getName();
923  const moveit::core::LinkModel* link_state = dfce->state_->getLinkModel(link_name);
924  if (updated_group_map.find(link_name) != updated_group_map.end())
925  {
926  continue;
927  }
928 
929  // populating array with link that are not part of the planning group
930  non_group_link_decompositions.push_back(getPosedLinkBodyPointDecomposition(link_state));
931  non_group_link_decompositions.back()->updatePose(dfce->state_->getFrameTransform(link_state->getName()));
932 
933  std::vector<const moveit::core::AttachedBody*> attached_bodies;
934  dfce->state_->getAttachedBodies(attached_bodies, link_state);
935  for (const moveit::core::AttachedBody* attached_body : attached_bodies)
936  {
937  non_group_attached_body_decompositions.push_back(
939  }
940  }
941  dfce->distance_field_ = std::make_shared<distance_field::PropagationDistanceField>(
942  size_.x(), size_.y(), size_.z(), resolution_, origin_.x() - 0.5 * size_.x(), origin_.y() - 0.5 * size_.y(),
944 
945  // ROS_INFO_STREAM("Creation took " <<
946  // (ros::WallTime::now()-before_create).toSec());
947  // TODO - deal with AllowedCollisionMatrix
948  // now we need to actually set the points
949  // TODO - deal with shifted robot
950  EigenSTL::vector_Vector3d all_points;
951  for (collision_detection::PosedBodyPointDecompositionPtr& non_group_link_decomposition :
952  non_group_link_decompositions)
953  {
954  all_points.insert(all_points.end(), non_group_link_decomposition->getCollisionPoints().begin(),
955  non_group_link_decomposition->getCollisionPoints().end());
956  }
957 
958  for (collision_detection::PosedBodyPointDecompositionVectorPtr& non_group_attached_body_decomposition :
959  non_group_attached_body_decompositions)
960  {
961  all_points.insert(all_points.end(), non_group_attached_body_decomposition->getCollisionPoints().begin(),
962  non_group_attached_body_decomposition->getCollisionPoints().end());
963  }
964 
965  dfce->distance_field_->addPointsToField(all_points);
966  ROS_DEBUG_STREAM("CollisionRobot distance field has been initialized with " << all_points.size() << " points.");
967  }
968  }
969  return dfce;
970 }
971 
973 {
974  const std::vector<const moveit::core::LinkModel*>& link_models = robot_model_->getLinkModelsWithCollisionGeometry();
975  for (const moveit::core::LinkModel* link_model : link_models)
976  {
977  if (link_model->getShapes().empty())
978  {
979  ROS_WARN("No collision geometry for link model %s though there should be", link_model->getName().c_str());
980  continue;
981  }
982 
983  ROS_DEBUG("Generating model for %s", link_model->getName().c_str());
984  BodyDecompositionConstPtr bd(new BodyDecomposition(link_model->getShapes(),
985  link_model->getCollisionOriginTransforms(), resolution,
986  getLinkPadding(link_model->getName())));
987  link_body_decomposition_vector_.push_back(bd);
988  link_body_decomposition_index_map_[link_model->getName()] = link_body_decomposition_vector_.size() - 1;
989  }
990 }
991 
993  visualization_msgs::MarkerArray& model_markers) const
994 {
995  // creating colors
996  std_msgs::ColorRGBA robot_color;
997  robot_color.r = 0;
998  robot_color.b = 0.8f;
999  robot_color.g = 0;
1000  robot_color.a = 0.5;
1001 
1002  std_msgs::ColorRGBA world_links_color;
1003  world_links_color.r = 1;
1004  world_links_color.g = 1;
1005  world_links_color.b = 0;
1006  world_links_color.a = 0.5;
1007 
1008  // creating sphere marker
1009  visualization_msgs::Marker sphere_marker;
1010  sphere_marker.header.frame_id = robot_model_->getRootLinkName();
1011  sphere_marker.header.stamp = ros::Time(0);
1012  sphere_marker.ns = distance_field_cache_entry_->group_name_ + "_sphere_decomposition";
1013  sphere_marker.id = 0;
1014  sphere_marker.type = visualization_msgs::Marker::SPHERE;
1015  sphere_marker.action = visualization_msgs::Marker::ADD;
1016  sphere_marker.pose.orientation.x = 0;
1017  sphere_marker.pose.orientation.y = 0;
1018  sphere_marker.pose.orientation.z = 0;
1019  sphere_marker.pose.orientation.w = 1;
1020  sphere_marker.color = robot_color;
1021  sphere_marker.lifetime = ros::Duration(0);
1022 
1023  unsigned int id = 0;
1025  const std::vector<std::string>& group_link_names = joint_group->getUpdatedLinkModelNames();
1026 
1027  std::map<std::string, unsigned int>::const_iterator map_iter;
1028  for (map_iter = link_body_decomposition_index_map_.begin(); map_iter != link_body_decomposition_index_map_.end();
1029  map_iter++)
1030  {
1031  const std::string& link_name = map_iter->first;
1032  unsigned int link_index = map_iter->second;
1033 
1034  // selecting color
1035  if (std::find(group_link_names.begin(), group_link_names.end(), link_name) != group_link_names.end())
1036  {
1037  sphere_marker.color = robot_color;
1038  }
1039  else
1040  {
1041  sphere_marker.color = world_links_color;
1042  }
1043 
1044  collision_detection::PosedBodySphereDecompositionPtr sphere_representation(
1046  sphere_representation->updatePose(state.getGlobalLinkTransform(link_name));
1047  for (unsigned int j = 0; j < sphere_representation->getCollisionSpheres().size(); j++)
1048  {
1049  sphere_marker.pose.position = tf2::toMsg(sphere_representation->getSphereCenters()[j]);
1050  sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z =
1051  2 * sphere_representation->getCollisionSpheres()[j].radius_;
1052  sphere_marker.id = id;
1053  id++;
1054 
1055  model_markers.markers.push_back(sphere_marker);
1056  }
1057  }
1058 }
1059 
1061  double resolution, const std::map<std::string, std::vector<CollisionSphere>>& link_spheres)
1062 {
1063  ROS_ASSERT_MSG(robot_model_, "RobotModelPtr is invalid");
1064  const std::vector<const moveit::core::LinkModel*>& link_models = robot_model_->getLinkModelsWithCollisionGeometry();
1065 
1066  for (const moveit::core::LinkModel* link_model : link_models)
1067  {
1068  if (link_model->getShapes().empty())
1069  {
1070  ROS_WARN_STREAM("Skipping model generation for link " << link_model->getName()
1071  << " since it contains no geometries");
1072  continue;
1073  }
1074 
1075  BodyDecompositionPtr bd(new BodyDecomposition(link_model->getShapes(), link_model->getCollisionOriginTransforms(),
1076  resolution, getLinkPadding(link_model->getName())));
1077 
1078  ROS_DEBUG("Generated model for %s", link_model->getName().c_str());
1079 
1080  if (link_spheres.find(link_model->getName()) != link_spheres.end())
1081  {
1082  bd->replaceCollisionSpheres(link_spheres.find(link_model->getName())->second, Eigen::Isometry3d::Identity());
1083  }
1084  link_body_decomposition_vector_.push_back(bd);
1085  link_body_decomposition_index_map_[link_model->getName()] = link_body_decomposition_vector_.size() - 1;
1086  }
1087  ROS_DEBUG_STREAM(__FUNCTION__ << " Finished ");
1088 }
1089 
1090 PosedBodySphereDecompositionPtr
1092  unsigned int ind) const
1093 {
1094  PosedBodySphereDecompositionPtr ret;
1095  ret = std::make_shared<PosedBodySphereDecomposition>(link_body_decomposition_vector_.at(ind));
1096  return ret;
1097 }
1098 
1099 PosedBodyPointDecompositionPtr
1101 {
1102  PosedBodyPointDecompositionPtr ret;
1103  std::map<std::string, unsigned int>::const_iterator it = link_body_decomposition_index_map_.find(ls->getName());
1104  if (it == link_body_decomposition_index_map_.end())
1105  {
1106  ROS_ERROR_NAMED("collision_distance_field", "No link body decomposition for link %s.", ls->getName().c_str());
1107  return ret;
1108  }
1109  ret = std::make_shared<PosedBodyPointDecomposition>(link_body_decomposition_vector_[it->second]);
1110  return ret;
1111 }
1112 
1114  GroupStateRepresentationPtr& gsr) const
1115 {
1116  for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++)
1117  {
1118  const moveit::core::LinkModel* ls = state.getLinkModel(gsr->dfce_->link_names_[i]);
1119  if (gsr->dfce_->link_has_geometry_[i])
1120  {
1121  gsr->link_body_decompositions_[i]->updatePose(state.getFrameTransform(ls->getName()));
1122  gsr->link_distance_fields_[i]->updatePose(state.getFrameTransform(ls->getName()));
1123  gsr->gradients_[i].closest_distance = DBL_MAX;
1124  gsr->gradients_[i].collision = false;
1125  gsr->gradients_[i].types.assign(gsr->link_body_decompositions_[i]->getCollisionSpheres().size(), NONE);
1126  gsr->gradients_[i].distances.assign(gsr->link_body_decompositions_[i]->getCollisionSpheres().size(), DBL_MAX);
1127  gsr->gradients_[i].gradients.assign(gsr->link_body_decompositions_[i]->getCollisionSpheres().size(),
1128  Eigen::Vector3d(0.0, 0.0, 0.0));
1129  gsr->gradients_[i].sphere_locations = gsr->link_body_decompositions_[i]->getSphereCenters();
1130  }
1131  }
1133  for (unsigned int i = 0; i < gsr->dfce_->attached_body_names_.size(); i++)
1134  {
1135  const moveit::core::AttachedBody* att = state.getAttachedBody(gsr->dfce_->attached_body_names_[i]);
1136  if (!att)
1137  {
1138  ROS_WARN("Attached body discrepancy");
1139  continue;
1140  }
1141 
1142  // TODO: This logic for checking attached body count might be incorrect
1143  if (gsr->attached_body_decompositions_.size() != att->getShapes().size())
1144  {
1145  ROS_WARN("Attached body size discrepancy");
1146  continue;
1147  }
1148 
1149  for (unsigned int j = 0; j < att->getShapes().size(); j++)
1150  {
1151  gsr->attached_body_decompositions_[i]->updatePose(j, att->getGlobalCollisionBodyTransforms()[j]);
1152  }
1153 
1154  gsr->gradients_[i + gsr->dfce_->link_names_.size()].closest_distance = DBL_MAX;
1155  gsr->gradients_[i + gsr->dfce_->link_names_.size()].collision = false;
1156  gsr->gradients_[i + gsr->dfce_->link_names_.size()].types.assign(
1157  gsr->attached_body_decompositions_[i]->getCollisionSpheres().size(), NONE);
1158  gsr->gradients_[i + gsr->dfce_->link_names_.size()].distances.assign(
1159  gsr->attached_body_decompositions_[i]->getCollisionSpheres().size(), DBL_MAX);
1160  gsr->gradients_[i + gsr->dfce_->link_names_.size()].gradients.assign(
1161  gsr->attached_body_decompositions_[i]->getCollisionSpheres().size(), Eigen::Vector3d(0.0, 0.0, 0.0));
1162  gsr->gradients_[i + gsr->dfce_->link_names_.size()].sphere_locations =
1163  gsr->attached_body_decompositions_[i]->getSphereCenters();
1164  }
1165 }
1166 
1167 void CollisionEnvDistanceField::getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr& dfce,
1168  const moveit::core::RobotState& state,
1169  GroupStateRepresentationPtr& gsr) const
1170 {
1171  if (!dfce->pregenerated_group_state_representation_)
1172  {
1173  ROS_DEBUG_STREAM("Creating GroupStateRepresentation");
1174 
1175  // unsigned int count = 0;
1176  gsr = std::make_shared<GroupStateRepresentation>();
1177  gsr->dfce_ = dfce;
1178  gsr->gradients_.resize(dfce->link_names_.size() + dfce->attached_body_names_.size());
1179 
1180  Eigen::Vector3d link_size;
1181  Eigen::Vector3d link_origin;
1182  for (unsigned int i = 0; i < dfce->link_names_.size(); i++)
1183  {
1184  const moveit::core::LinkModel* ls = state.getLinkModel(dfce->link_names_[i]);
1185  if (dfce->link_has_geometry_[i])
1186  {
1187  // create link body geometric decomposition
1188  gsr->link_body_decompositions_.push_back(getPosedLinkBodySphereDecomposition(ls, dfce->link_body_indices_[i]));
1189 
1190  // create and fill link distance field
1191  PosedBodySphereDecompositionPtr& link_bd = gsr->link_body_decompositions_.back();
1192  double diameter = 2 * link_bd->getBoundingSphereRadius();
1193  link_size = Eigen::Vector3d(diameter, diameter, diameter);
1194  link_origin = link_bd->getBoundingSphereCenter() - 0.5 * link_size;
1195 
1196  ROS_DEBUG_STREAM("Creating PosedDistanceField for link "
1197  << dfce->link_names_[i] << " with size [" << link_size.x() << ", " << link_size.y() << ", "
1198  << link_size.z() << "] and origin " << link_origin.x() << ", " << link_origin.y() << ", "
1199  << link_origin.z());
1200 
1201  gsr->link_distance_fields_.push_back(std::make_shared<PosedDistanceField>(
1203  gsr->link_distance_fields_.back()->addPointsToField(link_bd->getCollisionPoints());
1204  ROS_DEBUG_STREAM("Created PosedDistanceField for link " << dfce->link_names_[i] << " with "
1205  << link_bd->getCollisionPoints().size() << " points");
1206 
1207  gsr->link_body_decompositions_.back()->updatePose(state.getFrameTransform(ls->getName()));
1208  gsr->link_distance_fields_.back()->updatePose(state.getFrameTransform(ls->getName()));
1209  gsr->gradients_[i].types.resize(gsr->link_body_decompositions_.back()->getCollisionSpheres().size(), NONE);
1210  gsr->gradients_[i].distances.resize(gsr->link_body_decompositions_.back()->getCollisionSpheres().size(),
1211  DBL_MAX);
1212  gsr->gradients_[i].gradients.resize(gsr->link_body_decompositions_.back()->getCollisionSpheres().size());
1213  gsr->gradients_[i].sphere_radii = gsr->link_body_decompositions_.back()->getSphereRadii();
1214  gsr->gradients_[i].joint_name = ls->getParentJointModel()->getName();
1215  }
1216  else
1217  {
1218  gsr->link_body_decompositions_.push_back(PosedBodySphereDecompositionPtr());
1219  gsr->link_distance_fields_.push_back(PosedDistanceFieldPtr());
1220  }
1221  }
1222  }
1223  else
1224  {
1225  gsr = std::make_shared<GroupStateRepresentation>(*(dfce->pregenerated_group_state_representation_));
1226  gsr->dfce_ = dfce;
1227  gsr->gradients_.resize(dfce->link_names_.size() + dfce->attached_body_names_.size());
1228  for (unsigned int i = 0; i < dfce->link_names_.size(); i++)
1229  {
1230  const moveit::core::LinkModel* ls = state.getLinkModel(dfce->link_names_[i]);
1231  if (dfce->link_has_geometry_[i])
1232  {
1233  gsr->link_body_decompositions_[i]->updatePose(state.getFrameTransform(ls->getName()));
1234  gsr->link_distance_fields_[i]->updatePose(state.getFrameTransform(ls->getName()));
1235  gsr->gradients_[i].sphere_locations = gsr->link_body_decompositions_[i]->getSphereCenters();
1236  }
1237  }
1238  }
1239 
1240  for (unsigned int i = 0; i < dfce->attached_body_names_.size(); i++)
1241  {
1242  int link_index = dfce->attached_body_link_state_indices_[i];
1243  const moveit::core::LinkModel* ls =
1244  state.getJointModelGroup(gsr->dfce_->group_name_)->getUpdatedLinkModels()[link_index];
1245  // const moveit::core::LinkModel* ls =
1246  // state.getLinkStateVector()[dfce->attached_body_link_state_indices_[i]];
1249  gsr->attached_body_decompositions_.push_back(
1250  getAttachedBodySphereDecomposition(state.getAttachedBody(dfce->attached_body_names_[i]), resolution_));
1251  gsr->gradients_[i + dfce->link_names_.size()].types.resize(
1252  gsr->attached_body_decompositions_.back()->getCollisionSpheres().size(), NONE);
1253  gsr->gradients_[i + dfce->link_names_.size()].distances.resize(
1254  gsr->attached_body_decompositions_.back()->getCollisionSpheres().size(), DBL_MAX);
1255  gsr->gradients_[i + dfce->link_names_.size()].gradients.resize(
1256  gsr->attached_body_decompositions_.back()->getCollisionSpheres().size());
1257  gsr->gradients_[i + dfce->link_names_.size()].sphere_locations =
1258  gsr->attached_body_decompositions_.back()->getSphereCenters();
1259  gsr->gradients_[i + dfce->link_names_.size()].sphere_radii =
1260  gsr->attached_body_decompositions_.back()->getSphereRadii();
1261  gsr->gradients_[i + dfce->link_names_.size()].joint_name = ls->getParentJointModel()->getName();
1262  }
1263 }
1264 
1265 bool CollisionEnvDistanceField::compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr& dfce,
1266  const moveit::core::RobotState& state) const
1267 {
1268  std::vector<double> new_state_values(state.getVariableCount());
1269  for (unsigned int i = 0; i < new_state_values.size(); i++)
1270  {
1271  new_state_values[i] = state.getVariablePosition(i);
1272  }
1273 
1274  if (dfce->state_values_.size() != new_state_values.size())
1275  {
1276  ROS_ERROR("State value size mismatch");
1277  return false;
1278  }
1279 
1280  for (unsigned int i = 0; i < dfce->state_check_indices_.size(); i++)
1281  {
1282  double diff =
1283  fabs(dfce->state_values_[dfce->state_check_indices_[i]] - new_state_values[dfce->state_check_indices_[i]]);
1284  if (diff > EPSILON)
1285  {
1286  ROS_WARN_STREAM("State for Variable " << state.getVariableNames()[dfce->state_check_indices_[i]]
1287  << " has changed by " << diff << " radians");
1288  return false;
1289  }
1290  }
1291  std::vector<const moveit::core::AttachedBody*> attached_bodies_dfce;
1292  std::vector<const moveit::core::AttachedBody*> attached_bodies_state;
1293  dfce->state_->getAttachedBodies(attached_bodies_dfce);
1294  state.getAttachedBodies(attached_bodies_state);
1295  if (attached_bodies_dfce.size() != attached_bodies_state.size())
1296  {
1297  return false;
1298  }
1299  // TODO - figure all the things that can change
1300  for (unsigned int i = 0; i < attached_bodies_dfce.size(); i++)
1301  {
1302  if (attached_bodies_dfce[i]->getName() != attached_bodies_state[i]->getName())
1303  {
1304  return false;
1305  }
1306  if (attached_bodies_dfce[i]->getTouchLinks() != attached_bodies_state[i]->getTouchLinks())
1307  {
1308  return false;
1309  }
1310  if (attached_bodies_dfce[i]->getShapes().size() != attached_bodies_state[i]->getShapes().size())
1311  {
1312  return false;
1313  }
1314  for (unsigned int j = 0; j < attached_bodies_dfce[i]->getShapes().size(); j++)
1315  {
1316  if (attached_bodies_dfce[i]->getShapes()[j] != attached_bodies_state[i]->getShapes()[j])
1317  {
1318  return false;
1319  }
1320  }
1321  }
1322  return true;
1323 }
1324 
1326  const DistanceFieldCacheEntryConstPtr& dfce, const collision_detection::AllowedCollisionMatrix& acm) const
1327 {
1328  if (dfce->acm_.getSize() != acm.getSize())
1329  {
1330  ROS_DEBUG("Allowed collision matrix size mismatch");
1331  return false;
1332  }
1333  std::vector<const moveit::core::AttachedBody*> attached_bodies;
1334  dfce->state_->getAttachedBodies(attached_bodies);
1335  for (unsigned int i = 0; i < dfce->link_names_.size(); i++)
1336  {
1337  std::string link_name = dfce->link_names_[i];
1338  if (dfce->link_has_geometry_[i])
1339  {
1340  bool self_collision_enabled = true;
1342  if (acm.getEntry(link_name, link_name, t))
1343  {
1345  {
1346  self_collision_enabled = false;
1347  }
1348  }
1349  if (self_collision_enabled != dfce->self_collision_enabled_[i])
1350  {
1351  // ROS_INFO_STREAM("Self collision for " << link_name << " went from "
1352  // << dfce->self_collision_enabled_[i] << " to " <<
1353  // self_collision_enabled);
1354  return false;
1355  }
1356  for (unsigned int j = i; j < dfce->link_names_.size(); j++)
1357  {
1358  if (i == j)
1359  continue;
1360  if (dfce->link_has_geometry_[j])
1361  {
1362  bool intra_collision_enabled = true;
1363  if (acm.getEntry(link_name, dfce->link_names_[j], t))
1364  {
1366  {
1367  intra_collision_enabled = false;
1368  }
1369  }
1370  if (dfce->intra_group_collision_enabled_[i][j] != intra_collision_enabled)
1371  {
1372  // std::cerr << "Intra collision for " << dfce->link_names_[i] << "
1373  // " << dfce->link_names_[j]
1374  // << " went from " <<
1375  // dfce->intra_group_collision_enabled_[i][j] << " to " <<
1376  // intra_collision_enabled << std::endl;
1377  return false;
1378  }
1379  }
1380  }
1381  }
1382  }
1383  return true;
1384 }
1385 
1386 // void
1387 // CollisionEnvDistanceField::generateAllowedCollisionInformation(CollisionEnvDistanceField::DistanceFieldCacheEntryPtr&
1388 // dfce)
1389 // {
1390 // for(unsigned int i = 0; i < dfce.link_names_.size(); i++) {
1391 // for(unsigned int j = 0; j <
1392 // if(dfce->acm.find
1393 // }
1394 // }
1395 
1396 void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res,
1397  const moveit::core::RobotState& state) const
1398 {
1399  GroupStateRepresentationPtr gsr;
1400  checkCollision(req, res, state, gsr);
1401 }
1402 
1403 void CollisionEnvDistanceField::checkCollision(const CollisionRequest& req, CollisionResult& res,
1404  const moveit::core::RobotState& state,
1405  GroupStateRepresentationPtr& gsr) const
1406 {
1407  if (!gsr)
1408  {
1409  generateCollisionCheckingStructures(req.group_name, state, nullptr, gsr, true);
1410  }
1411  else
1412  {
1414  }
1415  bool done = getSelfCollisions(req, res, gsr);
1416  if (!done)
1417  {
1418  done = getIntraGroupCollisions(req, res, gsr);
1419  }
1420  if (!done)
1421  {
1422  getEnvironmentCollisions(req, res, distance_field_cache_entry_world_->distance_field_, gsr);
1423  }
1424 
1425  (const_cast<CollisionEnvDistanceField*>(this))->last_gsr_ = gsr;
1426 }
1427 
1429  const moveit::core::RobotState& state,
1430  const AllowedCollisionMatrix& acm) const
1431 {
1432  GroupStateRepresentationPtr gsr;
1433  checkCollision(req, res, state, acm, gsr);
1434 }
1437  const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm,
1438  GroupStateRepresentationPtr& gsr) const
1439 {
1440  if (!gsr)
1441  {
1442  generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true);
1443  }
1444  else
1445  {
1447  }
1448  bool done = getSelfCollisions(req, res, gsr);
1449  if (!done)
1450  {
1451  done = getIntraGroupCollisions(req, res, gsr);
1452  }
1453  if (!done)
1454  {
1455  getEnvironmentCollisions(req, res, distance_field_cache_entry_world_->distance_field_, gsr);
1456  }
1457 
1458  (const_cast<CollisionEnvDistanceField*>(this))->last_gsr_ = gsr;
1459 }
1462  const moveit::core::RobotState& state) const
1463 {
1464  GroupStateRepresentationPtr gsr;
1465  checkRobotCollision(req, res, state, gsr);
1466 }
1467 
1469  const moveit::core::RobotState& state,
1470  GroupStateRepresentationPtr& gsr) const
1471 {
1472  distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_world_->distance_field_;
1473  if (!gsr)
1474  {
1475  generateCollisionCheckingStructures(req.group_name, state, nullptr, gsr, false);
1476  }
1477  else
1478  {
1480  }
1481  getEnvironmentCollisions(req, res, env_distance_field, gsr);
1482  (const_cast<CollisionEnvDistanceField*>(this))->last_gsr_ = gsr;
1483 
1484  // checkRobotCollisionHelper(req, res, robot, state, &acm);
1485 }
1486 
1488  const moveit::core::RobotState& state,
1489  const AllowedCollisionMatrix& acm) const
1490 {
1491  GroupStateRepresentationPtr gsr;
1492  checkRobotCollision(req, res, state, acm, gsr);
1494 
1496  const moveit::core::RobotState& state,
1497  const AllowedCollisionMatrix& acm,
1498  GroupStateRepresentationPtr& gsr) const
1499 {
1500  distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_world_->distance_field_;
1501 
1502  if (!gsr)
1503  {
1504  generateCollisionCheckingStructures(req.group_name, state, &acm, gsr, true);
1505  }
1506  else
1507  {
1509  }
1510  getEnvironmentCollisions(req, res, env_distance_field, gsr);
1511  (const_cast<CollisionEnvDistanceField*>(this))->last_gsr_ = gsr;
1512 
1513  // checkRobotCollisionHelper(req, res, robot, state, &acm);
1514 }
1515 
1516 void CollisionEnvDistanceField::checkRobotCollision(const CollisionRequest& /*req*/, CollisionResult& /*res*/,
1517  const moveit::core::RobotState& /*state1*/,
1518  const moveit::core::RobotState& /*state2*/,
1519  const AllowedCollisionMatrix& /*acm*/) const
1520 {
1521  ROS_ERROR_NAMED("collision_detection.distance", "Continuous collision checking not implemented");
1522 }
1523 
1525  const moveit::core::RobotState& /*state1*/,
1526  const moveit::core::RobotState& /*state2*/) const
1528  ROS_ERROR_NAMED("collision_detection.distance", "Continuous collision checking not implemented");
1529 }
1530 
1532  const moveit::core::RobotState& state,
1533  const AllowedCollisionMatrix* acm,
1534  GroupStateRepresentationPtr& gsr) const
1535 {
1536  distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_world_->distance_field_;
1537 
1538  if (!gsr)
1539  {
1540  generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true);
1541  }
1542  else
1543  {
1545  }
1546 
1549  getEnvironmentProximityGradients(env_distance_field, gsr);
1550 
1551  (const_cast<CollisionEnvDistanceField*>(this))->last_gsr_ = gsr;
1552 }
1553 
1555  const moveit::core::RobotState& state,
1557  GroupStateRepresentationPtr& gsr) const
1558 {
1559  if (!gsr)
1560  {
1561  generateCollisionCheckingStructures(req.group_name, state, acm, gsr, true);
1562  }
1563  else
1564  {
1566  }
1567  getSelfCollisions(req, res, gsr);
1568  getIntraGroupCollisions(req, res, gsr);
1569  distance_field::DistanceFieldConstPtr env_distance_field = distance_field_cache_entry_world_->distance_field_;
1570  getEnvironmentCollisions(req, res, env_distance_field, gsr);
1571 
1572  (const_cast<CollisionEnvDistanceField*>(this))->last_gsr_ = gsr;
1573 }
1574 
1575 bool CollisionEnvDistanceField::getEnvironmentCollisions(const CollisionRequest& req, CollisionResult& res,
1576  const distance_field::DistanceFieldConstPtr& env_distance_field,
1577  GroupStateRepresentationPtr& gsr) const
1578 {
1579  for (unsigned int i = 0; i < gsr->dfce_->link_names_.size() + gsr->dfce_->attached_body_names_.size(); i++)
1580  {
1581  bool is_link = i < gsr->dfce_->link_names_.size();
1582  std::string link_name = i < gsr->dfce_->link_names_.size() ? gsr->dfce_->link_names_[i] : "attached";
1583  if (is_link && !gsr->dfce_->link_has_geometry_[i])
1584  {
1585  continue;
1586  }
1587 
1588  const std::vector<CollisionSphere>* collision_spheres_1;
1589  const EigenSTL::vector_Vector3d* sphere_centers_1;
1590 
1591  if (is_link)
1592  {
1593  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
1594  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
1595  }
1596  else
1597  {
1598  collision_spheres_1 =
1599  &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
1600  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
1601  }
1602 
1603  if (req.contacts)
1604  {
1605  std::vector<unsigned int> colls;
1606  bool coll =
1607  getCollisionSphereCollision(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1,
1609  std::min(req.max_contacts_per_pair, req.max_contacts - res.contact_count), colls);
1610  if (coll)
1611  {
1612  res.collision = true;
1613  for (unsigned int col : colls)
1614  {
1615  Contact con;
1616  if (is_link)
1617  {
1618  con.pos = gsr->link_body_decompositions_[i]->getSphereCenters()[col];
1620  con.body_name_1 = gsr->dfce_->link_names_[i];
1621  }
1622  else
1623  {
1624  con.pos = gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters()[col];
1626  con.body_name_1 = gsr->dfce_->attached_body_names_[i - gsr->dfce_->link_names_.size()];
1627  }
1628 
1630  con.body_name_2 = "environment";
1631  res.contact_count++;
1632  res.contacts[std::pair<std::string, std::string>(con.body_name_1, con.body_name_2)].push_back(con);
1633  gsr->gradients_[i].types[col] = ENVIRONMENT;
1634  // ROS_DEBUG_STREAM("Link " << dfce->link_names_[i] << " sphere " <<
1635  // colls[j] << " in env collision");
1636  }
1637 
1638  gsr->gradients_[i].collision = true;
1639  if (res.contact_count >= req.max_contacts)
1640  {
1641  return true;
1642  }
1643  }
1644  }
1645  else
1646  {
1647  bool coll = getCollisionSphereCollision(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1,
1649  if (coll)
1650  {
1651  res.collision = true;
1652  return true;
1653  }
1654  }
1655  }
1656  return (res.contact_count >= req.max_contacts);
1657 }
1658 
1660  const distance_field::DistanceFieldConstPtr& env_distance_field, GroupStateRepresentationPtr& gsr) const
1661 {
1662  bool in_collision = false;
1663  for (unsigned int i = 0; i < gsr->dfce_->link_names_.size(); i++)
1664  {
1665  bool is_link = i < gsr->dfce_->link_names_.size();
1666 
1667  if (is_link && !gsr->dfce_->link_has_geometry_[i])
1668  {
1669  continue;
1670  }
1671 
1672  const std::vector<CollisionSphere>* collision_spheres_1;
1673  const EigenSTL::vector_Vector3d* sphere_centers_1;
1674  if (is_link)
1675  {
1676  collision_spheres_1 = &(gsr->link_body_decompositions_[i]->getCollisionSpheres());
1677  sphere_centers_1 = &(gsr->link_body_decompositions_[i]->getSphereCenters());
1678  }
1679  else
1680  {
1681  collision_spheres_1 =
1682  &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getCollisionSpheres());
1683  sphere_centers_1 = &(gsr->attached_body_decompositions_[i - gsr->dfce_->link_names_.size()]->getSphereCenters());
1684  }
1685 
1686  bool coll = getCollisionSphereGradients(env_distance_field.get(), *collision_spheres_1, *sphere_centers_1,
1687  gsr->gradients_[i], ENVIRONMENT, collision_tolerance_, false,
1688  max_propogation_distance_, false);
1689  if (coll)
1690  {
1691  in_collision = true;
1692  }
1693  }
1694  return in_collision;
1695 }
1696 
1697 void CollisionEnvDistanceField::setWorld(const WorldPtr& world)
1698 {
1699  if (world == getWorld())
1700  return;
1701 
1702  // turn off notifications about old world
1703  getWorld()->removeObserver(observer_handle_);
1704 
1705  // clear out objects from old world
1706  distance_field_cache_entry_world_->distance_field_->reset();
1707 
1708  CollisionEnv::setWorld(world);
1709 
1710  // request notifications about changes to new world
1711  observer_handle_ = getWorld()->addObserver(
1712  std::bind(&CollisionEnvDistanceField::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
1713 
1714  // get notifications any objects already in the new world
1715  getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);
1716 }
1717 
1718 void CollisionEnvDistanceField::notifyObjectChange(CollisionEnvDistanceField* self, const ObjectConstPtr& obj,
1719  World::Action action)
1720 {
1722 
1723  EigenSTL::vector_Vector3d add_points;
1724  EigenSTL::vector_Vector3d subtract_points;
1725  self->updateDistanceObject(obj->id_, self->distance_field_cache_entry_world_, add_points, subtract_points);
1726 
1727  if (action == World::DESTROY)
1728  {
1729  self->distance_field_cache_entry_world_->distance_field_->removePointsFromField(subtract_points);
1730  }
1731  else if (action & (World::MOVE_SHAPE | World::REMOVE_SHAPE))
1732  {
1733  self->distance_field_cache_entry_world_->distance_field_->removePointsFromField(subtract_points);
1734  self->distance_field_cache_entry_world_->distance_field_->addPointsToField(add_points);
1735  }
1736  else
1737  {
1738  self->distance_field_cache_entry_world_->distance_field_->addPointsToField(add_points);
1739  }
1740 
1741  ROS_DEBUG_NAMED("collision_distance_field", "Modifying object %s took %lf s", obj->id_.c_str(),
1742  (ros::WallTime::now() - n).toSec());
1743 }
1744 
1745 void CollisionEnvDistanceField::updateDistanceObject(const std::string& id, DistanceFieldCacheEntryWorldPtr& dfce,
1746  EigenSTL::vector_Vector3d& add_points,
1747  EigenSTL::vector_Vector3d& subtract_points)
1748 {
1749  std::map<std::string, std::vector<PosedBodyPointDecompositionPtr>>::iterator cur_it =
1750  dfce->posed_body_point_decompositions_.find(id);
1751  if (cur_it != dfce->posed_body_point_decompositions_.end())
1752  {
1753  for (collision_detection::PosedBodyPointDecompositionPtr& posed_body_point_decomposition : cur_it->second)
1754  {
1755  subtract_points.insert(subtract_points.end(), posed_body_point_decomposition->getCollisionPoints().begin(),
1756  posed_body_point_decomposition->getCollisionPoints().end());
1757  }
1758  }
1759 
1760  World::ObjectConstPtr object = getWorld()->getObject(id);
1761  if (object)
1762  {
1763  ROS_DEBUG_STREAM("Updating/Adding Object '" << object->id_ << "' with " << object->shapes_.size()
1764  << " shapes to CollisionEnvDistanceField");
1765  std::vector<PosedBodyPointDecompositionPtr> shape_points;
1766  for (unsigned int i = 0; i < object->shapes_.size(); i++)
1767  {
1768  shapes::ShapeConstPtr shape = object->shapes_[i];
1769  if (shape->type == shapes::OCTREE)
1770  {
1771  const shapes::OcTree* octree_shape = static_cast<const shapes::OcTree*>(shape.get());
1772  std::shared_ptr<const octomap::OcTree> octree = octree_shape->octree;
1773 
1774  shape_points.push_back(std::make_shared<PosedBodyPointDecomposition>(octree));
1775  }
1776  else
1777  {
1778  BodyDecompositionConstPtr bd = getBodyDecompositionCacheEntry(shape, resolution_);
1779  shape_points.push_back(
1780  std::make_shared<PosedBodyPointDecomposition>(bd, getWorld()->getGlobalShapeTransform(id, i)));
1781  }
1782 
1783  add_points.insert(add_points.end(), shape_points.back()->getCollisionPoints().begin(),
1784  shape_points.back()->getCollisionPoints().end());
1785  }
1786 
1787  dfce->posed_body_point_decompositions_[id] = shape_points;
1788  }
1789  else
1790  {
1791  ROS_DEBUG_STREAM("Removing Object '" << id << "' from CollisionEnvDistanceField");
1792  dfce->posed_body_point_decompositions_.erase(id);
1793  }
1794 }
1795 
1796 CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr
1798 {
1799  DistanceFieldCacheEntryWorldPtr dfce(new DistanceFieldCacheEntryWorld());
1800  dfce->distance_field_ = std::make_shared<distance_field::PropagationDistanceField>(
1801  size_.x(), size_.y(), size_.z(), resolution_, origin_.x() - 0.5 * size_.x(), origin_.y() - 0.5 * size_.y(),
1803 
1804  EigenSTL::vector_Vector3d add_points;
1805  EigenSTL::vector_Vector3d subtract_points;
1806  for (const std::pair<const std::string, ObjectPtr>& object : *getWorld())
1807  {
1808  updateDistanceObject(object.first, dfce, add_points, subtract_points);
1809  }
1810  dfce->distance_field_->addPointsToField(add_points);
1811  return dfce;
1812 }
1813 
1814 const std::string& CollisionDetectorAllocatorDistanceField::getName() const
1815 {
1816  return NAME;
1817 }
1818 
1819 } // namespace collision_detection
collision_detection::INTRA
@ INTRA
Definition: collision_distance_field_types.h:125
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
collision_detection::CollisionEnvDistanceField::observer_handle_
World::ObserverHandle observer_handle_
Definition: collision_env_distance_field.h:337
collision_common_distance_field.h
collision_detection::Contact::body_type_1
BodyType body_type_1
The type of the first body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:122
collision_detection::World::DESTROY
@ DESTROY
Definition: world.h:256
initialize
bool initialize(MeshCollisionTraversalNode< BV > &node, BVHModel< BV > &model1, Transform3< typename BV::S > &tf1, BVHModel< BV > &model2, Transform3< typename BV::S > &tf2, const CollisionRequest< typename BV::S > &request, CollisionResult< typename BV::S > &result, bool use_refit, bool refit_bottomup)
shapes::OcTree::octree
std::shared_ptr< const octomap::OcTree > octree
collision_detection::CollisionEnvDistanceField::~CollisionEnvDistanceField
~CollisionEnvDistanceField() override
Definition: collision_env_distance_field.cpp:150
moveit::core::RobotState::getVariableCount
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:176
EPSILON
constexpr double EPSILON
Definition: test_kinematic_complex.cpp:49
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
collision_detection::CollisionEnvDistanceField::updateGroupStateRepresentationState
void updateGroupStateRepresentationState(const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:1145
collision_detection::BodyTypes::WORLD_OBJECT
@ WORLD_OBJECT
A body in the environment.
Definition: include/moveit/collision_detection/collision_common.h:97
collision_detection::CollisionEnvDistanceField::use_signed_distance_field_
bool use_signed_distance_field_
Definition: collision_env_distance_field.h:319
tf2_eigen.h
collision_detection::AllowedCollisionMatrix::getSize
std::size_t getSize() const
Get the size of the allowed collision matrix (number of specified entries)
Definition: collision_matrix.h:231
collision_detection::CollisionEnvDistanceField::getPosedLinkBodyPointDecomposition
PosedBodyPointDecompositionPtr getPosedLinkBodyPointDecomposition(const moveit::core::LinkModel *ls) const
Definition: collision_env_distance_field.cpp:1132
propagation_distance_field.h
collision_detection::CollisionEnvDistanceField::distance_field_cache_entry_
DistanceFieldCacheEntryPtr distance_field_cache_entry_
Definition: collision_env_distance_field.h:328
moveit::core::RobotState::getLinkModel
const LinkModel * getLinkModel(const std::string &link) const
Get the model of a particular link.
Definition: robot_state.h:188
getName
ROSCONSOLE_CONSOLE_IMPL_DECL std::string getName(void *handle)
collision_detection::CollisionEnvDistanceField::checkSelfCollisionHelper
void checkSelfCollisionHelper(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:206
collision_detection::AllowedCollisionMatrix::getEntry
bool getEntry(const std::string &name1, const std::string &name2, AllowedCollision::Type &allowed_collision_type) const
Get the type of the allowed collision between two elements. Return true if the entry is included in t...
Definition: collision_matrix.cpp:120
moveit::core::JointModel::getName
const std::string & getName() const
Get the name of the joint.
Definition: joint_model.h:196
collision_detection::CollisionEnvDistanceField::generateDistanceFieldCacheEntry
DistanceFieldCacheEntryPtr generateDistanceFieldCacheEntry(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, bool generate_distance_field) const
Definition: collision_env_distance_field.cpp:752
ROS_DEBUG
#define ROS_DEBUG(...)
moveit::core::LinkModel::getParentJointModel
const JointModel * getParentJointModel() const
Get the joint model whose child this link is. There will always be a parent joint.
Definition: link_model.h:107
ROS_DEBUG_COND
#define ROS_DEBUG_COND(cond,...)
collision_detection::AllowedCollision::ALWAYS
@ ALWAYS
Collisions between a particular pair of bodies does not imply that the robot configuration is in coll...
Definition: collision_matrix.h:157
collision_detection::CollisionEnvDistanceField::getIntraGroupProximityGradients
bool getIntraGroupProximityGradients(GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:686
collision_detection::SELF
@ SELF
Definition: collision_distance_field_types.h:124
moveit::core::RobotState::getAttachedBody
const AttachedBody * getAttachedBody(const std::string &name) const
Get the attached body named name. Return NULL if not found.
Definition: robot_state.cpp:1045
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_env_distance_field.h
moveit::core::RobotState::getGlobalLinkTransform
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
Definition: robot_state.h:1460
collision_detection::CollisionEnvDistanceField::compareCacheEntryToState
bool compareCacheEntryToState(const DistanceFieldCacheEntryConstPtr &dfce, const moveit::core::RobotState &state) const
Definition: collision_env_distance_field.cpp:1297
collision_detection::CollisionEnvDistanceField::planning_scene_
planning_scene::PlanningScenePtr planning_scene_
Definition: collision_env_distance_field.h:332
collision_detection::Contact
Definition of a contact point.
Definition: include/moveit/collision_detection/collision_common.h:105
collision_detection::CollisionEnvDistanceField::checkCollision
void checkCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const override
Check whether the robot model is in collision with itself or the world at a particular state....
Definition: collision_env_distance_field.cpp:1428
ROS_DEBUG_STREAM
#define ROS_DEBUG_STREAM(args)
collision_detection::CollisionEnvDistanceField::pregenerated_group_state_representation_map_
std::map< std::string, GroupStateRepresentationPtr > pregenerated_group_state_representation_map_
Definition: collision_env_distance_field.h:330
obj
CollisionObject< S > * obj
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:216
collision_detection::getAttachedBodyPointDecomposition
PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const moveit::core::AttachedBody *att, double resolution)
Definition: collision_common_distance_field.cpp:149
robot_model.h
collision_detection::doBoundingSpheresIntersect
bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr &p1, const PosedBodySphereDecompositionConstPtr &p2)
Definition: collision_distance_field_types.cpp:396
moveit::core::RobotState::getVariableNames
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up this state, in the order they are stored in memory.
Definition: robot_state.h:182
collision_detection::Contact::body_name_2
std::string body_name_2
The id of the second body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:125
collision_detection::Contact::body_type_2
BodyType body_type_2
The type of the second body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:128
collision_detection::CollisionEnvDistanceField::getEnvironmentCollisions
bool getEnvironmentCollisions(const CollisionRequest &req, CollisionResult &res, const distance_field::DistanceFieldConstPtr &env_distance_field, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:1607
moveit::core::AttachedBody
Object defining bodies that can be attached to robot links.
Definition: attached_body.h:121
collision_detection::Contact::body_name_1
std::string body_name_1
The id of the first body involved in the contact.
Definition: include/moveit/collision_detection/collision_common.h:119
collision_detection::CollisionEnvDistanceField::distance_field_cache_entry_world_
DistanceFieldCacheEntryWorldPtr distance_field_cache_entry_world_
Definition: collision_env_distance_field.h:335
collision_detection::CollisionEnvDistanceField::size_
Eigen::Vector3d size_
Definition: collision_env_distance_field.h:317
console.h
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
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::CollisionEnvDistanceField::update_cache_lock_
boost::mutex update_cache_lock_
Definition: collision_env_distance_field.h:327
collision_detection::NAME
static const std::string NAME
Definition: collision_env_fcl.cpp:81
collision_detection::CollisionEnvDistanceField::getSelfProximityGradients
bool getSelfProximityGradients(GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:382
collision_detection::CollisionEnvDistanceField::collision_tolerance_
double collision_tolerance_
Definition: collision_env_distance_field.h:321
collision_detection::CollisionEnvDistanceField::compareCacheEntryToAllowedCollisionMatrix
bool compareCacheEntryToAllowedCollisionMatrix(const DistanceFieldCacheEntryConstPtr &dfce, const collision_detection::AllowedCollisionMatrix &acm) const
Definition: collision_env_distance_field.cpp:1357
collision_detection::CollisionEnv::setPadding
void setPadding(double padding)
Set the link padding (for every link)
Definition: collision_env.cpp:111
ROS_DEBUG_NAMED
#define ROS_DEBUG_NAMED(name,...)
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:177
collision_detection::AllowedCollision::Type
Type
Definition: collision_matrix.h:117
collision_detection::CollisionEnvDistanceField::in_group_update_map_
std::map< std::string, std::map< std::string, bool > > in_group_update_map_
Definition: collision_env_distance_field.h:329
ros::WallTime::now
static WallTime now()
collision_detection::getCollisionSphereGradients
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)
Definition: collision_distance_field_types.cpp:129
collision_detection::CollisionRequest::max_contacts
std::size_t max_contacts
Overall maximum number of contacts to compute.
Definition: include/moveit/collision_detection/collision_common.h:245
collision_detection::CollisionEnvDistanceField::link_body_decomposition_vector_
std::vector< BodyDecompositionConstPtr > link_body_decomposition_vector_
Definition: collision_env_distance_field.h:324
collision_detection::CollisionEnvDistanceField::setWorld
void setWorld(const WorldPtr &world) override
Definition: collision_env_distance_field.cpp:1729
collision_detection::CollisionEnvDistanceField::getPosedLinkBodySphereDecomposition
PosedBodySphereDecompositionPtr getPosedLinkBodySphereDecomposition(const moveit::core::LinkModel *ls, unsigned int ind) const
Definition: collision_env_distance_field.cpp:1123
collision_detection::CollisionEnvDistanceField
Definition: collision_env_distance_field.h:90
moveit::core::RobotState::getVariablePosition
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
Definition: robot_state.h:273
collision_detection::CollisionRequest::group_name
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot)
Definition: include/moveit/collision_detection/collision_common.h:233
collision_detection::CollisionEnvDistanceField::getAllCollisions
void getAllCollisions(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:1586
moveit::core::JointModelGroup::getUpdatedLinkModelNames
const std::vector< std::string > & getUpdatedLinkModelNames() const
Get the names of the links returned by getUpdatedLinkModels()
Definition: joint_model_group.h:311
collision_detection::getBodyDecompositionCacheEntry
BodyDecompositionConstPtr getBodyDecompositionCacheEntry(const shapes::ShapeConstPtr &shape, double resolution)
Definition: collision_common_distance_field.cpp:97
moveit::core::AttachedBody::getGlobalCollisionBodyTransforms
const EigenSTL::vector_Isometry3d & getGlobalCollisionBodyTransforms() const
Get the global transforms (in world frame) for the collision bodies. The returned transforms are guar...
Definition: attached_body.h:263
ROS_ERROR
#define ROS_ERROR(...)
ROS_ASSERT_MSG
#define ROS_ASSERT_MSG(cond,...)
collision_detection::NONE
@ NONE
Definition: collision_distance_field_types.h:123
ROS_WARN
#define ROS_WARN(...)
collision_detection::BodyTypes::ROBOT_ATTACHED
@ ROBOT_ATTACHED
A body attached to a robot link.
Definition: include/moveit/collision_detection/collision_common.h:94
collision_detection::CollisionEnvDistanceField::generateCollisionCheckingStructures
void generateCollisionCheckingStructures(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr, bool generate_distance_field) const
Definition: collision_env_distance_field.cpp:187
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:1078
collision_detection::CollisionEnvDistanceField::origin_
Eigen::Vector3d origin_
Definition: collision_env_distance_field.h:318
collision_detection::CollisionEnvDistanceField::getSelfCollisions
bool getSelfCollisions(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:301
collision_detection::ENVIRONMENT
@ ENVIRONMENT
Definition: collision_distance_field_types.h:126
ros::WallTime
collision_detection::BodyTypes::ROBOT_LINK
@ ROBOT_LINK
A link on the robot.
Definition: include/moveit/collision_detection/collision_common.h:91
shapes::OcTree
collision_detection::CollisionEnvDistanceField::link_body_decomposition_index_map_
std::map< std::string, unsigned int > link_body_decomposition_index_map_
Definition: collision_env_distance_field.h:325
shapes::OCTREE
OCTREE
collision_detection::CollisionDetectorAllocatorDistanceField::getName
const std::string & getName() const override
Definition: collision_env_distance_field.cpp:1846
collision_detection::CollisionResult::contacts
ContactMap contacts
A map returning the pairs of body ids in contact, plus their contact details.
Definition: include/moveit/collision_detection/collision_common.h:209
moveit::core::RobotState::updateLinkTransforms
void updateLinkTransforms()
Update the reference frame transforms for links. This call is needed before using the transforms of l...
Definition: robot_state.cpp:761
collision_detection::CollisionEnvDistanceField::getGroupStateRepresentation
void getGroupStateRepresentation(const DistanceFieldCacheEntryConstPtr &dfce, const moveit::core::RobotState &state, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:1199
collision_detector_allocator_distance_field.h
collision_detection::Contact::pos
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Vector3d pos
contact position
Definition: include/moveit/collision_detection/collision_common.h:110
collision_detection::getCollisionSphereCollision
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)
Definition: collision_distance_field_types.cpp:192
moveit::core::RobotState::getFrameTransform
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
Definition: robot_state.cpp:1165
collision_detection::CollisionEnvDistanceField::updateDistanceObject
void updateDistanceObject(const std::string &id, CollisionEnvDistanceField::DistanceFieldCacheEntryWorldPtr &dfce, EigenSTL::vector_Vector3d &add_points, EigenSTL::vector_Vector3d &subtract_points)
Definition: collision_env_distance_field.cpp:1777
ros::Time
collision_detection::World::MOVE_SHAPE
@ MOVE_SHAPE
Definition: world.h:257
collision_detection::CollisionEnvDistanceField::addLinkBodyDecompositions
void addLinkBodyDecompositions(double resolution)
Definition: collision_env_distance_field.cpp:1004
moveit::core::JointModelGroup::getUpdatedLinkModels
const std::vector< const LinkModel * > & getUpdatedLinkModels() const
Get the names of the links that are to be updated when the state of this group changes....
Definition: joint_model_group.h:299
collision_detection::CollisionEnvDistanceField::getDistanceFieldCacheEntry
DistanceFieldCacheEntryConstPtr getDistanceFieldCacheEntry(const std::string &group_name, const moveit::core::RobotState &state, const collision_detection::AllowedCollisionMatrix *acm) const
Definition: collision_env_distance_field.cpp:231
collision_detection::CollisionRequest::contacts
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
Definition: include/moveit/collision_detection/collision_common.h:242
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::CollisionEnvDistanceField::getCollisionGradients
void getCollisionGradients(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state, const AllowedCollisionMatrix *acm, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:1563
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::CollisionEnvDistanceField::initialize
void initialize(const std::map< std::string, std::vector< CollisionSphere >> &link_body_decompositions, const Eigen::Vector3d &size, const Eigen::Vector3d &origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance)
Definition: collision_env_distance_field.cpp:155
collision_detection::CollisionEnvDistanceField::CollisionEnvDistanceField
EIGEN_MAKE_ALIGNED_OPERATOR_NEW CollisionEnvDistanceField(const moveit::core::RobotModelConstPtr &robot_model, const std::map< std::string, std::vector< CollisionSphere >> &link_body_decompositions=std::map< std::string, std::vector< CollisionSphere >>(), double size_x=DEFAULT_SIZE_X, double size_y=DEFAULT_SIZE_Y, double size_z=DEFAULT_SIZE_Z, const 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, double padding=0.0, double scale=1.0)
Definition: collision_env_distance_field.cpp:89
tf2::toMsg
B toMsg(const A &a)
collision_detection::CollisionEnvDistanceField::getEnvironmentProximityGradients
bool getEnvironmentProximityGradients(const distance_field::DistanceFieldConstPtr &env_distance_field, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:1691
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
collision_detection::CollisionResult::collision
bool collision
True if collision was found, false otherwise.
Definition: include/moveit/collision_detection/collision_common.h:200
collision_detection::CollisionEnvDistanceField::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_distance_field.cpp:1493
collision_detection::CollisionEnvDistanceField::getIntraGroupCollisions
bool getIntraGroupCollisions(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res, GroupStateRepresentationPtr &gsr) const
Definition: collision_env_distance_field.cpp:457
collision_detection::PosedBodySphereDecomposition
Definition: collision_distance_field_types.h:325
collision_detection::CollisionEnvDistanceField::generateDistanceFieldCacheEntryWorld
DistanceFieldCacheEntryWorldPtr generateDistanceFieldCacheEntryWorld()
Definition: collision_env_distance_field.cpp:1829
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::CollisionRequest::max_contacts_per_pair
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...
Definition: include/moveit/collision_detection/collision_common.h:249
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
moveit::core::RobotState::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &group) const
Get the model of a particular joint group.
Definition: robot_state.h:200
assert.h
collision_detection::CollisionEnvDistanceField::max_propogation_distance_
double max_propogation_distance_
Definition: collision_env_distance_field.h:322
collision_detection::CollisionResult::contact_count
std::size_t contact_count
Number of contacts returned.
Definition: include/moveit/collision_detection/collision_common.h:206
ros::Duration
collision_detection::CollisionEnv::robot_model_
moveit::core::RobotModelConstPtr robot_model_
The kinematic model corresponding to this collision model.
Definition: collision_env.h:347
collision_detection::getAttachedBodySphereDecomposition
PosedBodySphereDecompositionVectorPtr getAttachedBodySphereDecomposition(const moveit::core::AttachedBody *att, double resolution)
Definition: collision_common_distance_field.cpp:135
collision_detection::CollisionEnvDistanceField::resolution_
double resolution_
Definition: collision_env_distance_field.h:320
collision_detection::CollisionEnvDistanceField::checkSelfCollision
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::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_distance_field.cpp:262
collision_detection::CollisionEnvDistanceField::notifyObjectChange
static void notifyObjectChange(CollisionEnvDistanceField *self, const ObjectConstPtr &obj, World::Action action)
Definition: collision_env_distance_field.cpp:1750
t
geometry_msgs::TransformStamped t
collision_detection::CollisionEnv
Provides the interface to the individual collision checking libraries.
Definition: collision_env.h:83
collision_detection::CollisionEnvDistanceField::createCollisionModelMarker
void createCollisionModelMarker(const moveit::core::RobotState &state, visualization_msgs::MarkerArray &model_markers) const
Definition: collision_env_distance_field.cpp:1024
moveit::core::JointModel
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:173
collision_detection::AllowedCollision::NEVER
@ NEVER
Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in a particular...
Definition: collision_matrix.h:153
collision_detection::World::REMOVE_SHAPE
@ REMOVE_SHAPE
Definition: world.h:259
collision_detection::CollisionEnv::getWorld
const WorldPtr & getWorld()
Definition: collision_env.h:272
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
collision_detection::CollisionEnvBullet::observer_handle_
World::ObserverHandle observer_handle_
Definition: collision_env_bullet.h:210
collision_detection
Definition: collision_detector_allocator_allvalid.h:42


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Nov 21 2021 03:25:56