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