compute_default_collisions.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 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: Dave Coleman */
36 
38 #include <boost/math/special_functions/binomial.hpp> // for statistics at end
39 #include <boost/thread.hpp>
40 #include <boost/lexical_cast.hpp>
41 #include <boost/unordered_map.hpp>
42 #include <boost/assign.hpp>
43 #include <ros/console.h>
44 
45 namespace moveit_setup_assistant
46 {
47 // ******************************************************************************************
48 // Custom Types, Enums and Structs
49 // ******************************************************************************************
50 
51 // Boost mapping of reasons for disabling a link pair to strings
52 const boost::unordered_map<DisabledReason, std::string> reasonsToString = boost::assign::map_list_of(NEVER, "Never")(
53  DEFAULT, "Default")(ADJACENT, "Adjacent")(ALWAYS, "Always")(USER, "User")(NOT_DISABLED, "Not Disabled");
54 
55 const boost::unordered_map<std::string, DisabledReason> reasonsFromString = boost::assign::map_list_of("Never", NEVER)(
56  "Default", DEFAULT)("Adjacent", ADJACENT)("Always", ALWAYS)("User", USER)("Not Disabled", NOT_DISABLED);
57 
58 // Unique set of pairs of links in string-based form
59 typedef std::set<std::pair<std::string, std::string> > StringPairSet;
60 
61 // Struct for passing parameters to threads, for cleaner code
63 {
65  int thread_id, int num_trials, StringPairSet* links_seen_colliding, boost::mutex* lock,
66  unsigned int* progress)
67  : scene_(scene)
68  , req_(req)
69  , thread_id_(thread_id)
70  , num_trials_(num_trials)
71  , links_seen_colliding_(links_seen_colliding)
72  , lock_(lock)
73  , progress_(progress)
74  {
75  }
79  unsigned int num_trials_;
80  StringPairSet* links_seen_colliding_;
81  boost::mutex* lock_;
82  unsigned int* progress_; // only to be updated by thread 0
83 };
84 
85 // LinkGraph defines a Link's model and a set of unique links it connects
86 typedef std::map<const robot_model::LinkModel*, std::set<const robot_model::LinkModel*> > LinkGraph;
87 
88 // ******************************************************************************************
89 // Static Prototypes
90 // ******************************************************************************************
91 
100 static bool setLinkPair(const std::string& linkA, const std::string& linkB, const DisabledReason reason,
101  LinkPairMap& link_pairs);
102 
108 static void computeConnectionGraph(const robot_model::LinkModel* link, LinkGraph& link_graph);
109 
115 static void computeConnectionGraphRec(const robot_model::LinkModel* link, LinkGraph& link_graph);
116 
124 static unsigned int disableAdjacentLinks(planning_scene::PlanningScene& scene, LinkGraph& link_graph,
125  LinkPairMap& link_pairs);
126 
134 static unsigned int disableDefaultCollisions(planning_scene::PlanningScene& scene, LinkPairMap& link_pairs,
136 
147 static unsigned int disableAlwaysInCollision(planning_scene::PlanningScene& scene, LinkPairMap& link_pairs,
149  StringPairSet& links_seen_colliding, double min_collision_faction = 0.95);
150 
159 static unsigned int disableNeverInCollision(const unsigned int num_trials, planning_scene::PlanningScene& scene,
160  LinkPairMap& link_pairs, const collision_detection::CollisionRequest& req,
161  StringPairSet& links_seen_colliding, unsigned int* progress);
162 
168 
169 // ******************************************************************************************
170 // Generates an adjacency list of links that are always and never in collision, to speed up collision detection
171 // ******************************************************************************************
172 LinkPairMap computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr& parent_scene, unsigned int* progress,
173  const bool include_never_colliding, const unsigned int num_trials,
174  const double min_collision_fraction, const bool verbose)
175 {
176  // Create new instance of planning scene using pointer
177  planning_scene::PlanningScenePtr scene = parent_scene->diff();
178 
179  // Map of disabled collisions that contains a link as a key and an ordered list of links that are connected.
180  LinkPairMap link_pairs;
181 
182  // Track unique edges that have been found to be in collision in some state
183  StringPairSet links_seen_colliding;
184 
185  // LinkGraph is a custom type of a map with a LinkModel as key and a set of LinkModels as second
186  LinkGraph link_graph;
187 
188  // ROS_INFO_STREAM("Initial allowed Collision Matrix Size = " << scene.getAllowedCollisions().getSize() );
189 
190  // 0. GENERATE ALL POSSIBLE LINK PAIRS -------------------------------------------------------------
191  // Generate a list of unique link pairs for all links with geometry. Order pairs alphabetically.
192  // There should be n choose 2 pairs
193  computeLinkPairs(*scene, link_pairs);
194  *progress = 1;
195 
196  // 1. FIND CONNECTING LINKS ------------------------------------------------------------------------
197  // For each link, compute the set of other links it connects to via a single joint (adjacent links)
198  // or via a chain of joints with intermediate links with no geometry (like a socket joint)
199 
200  // Create Connection Graph
201  computeConnectionGraph(scene->getRobotModel()->getRootLink(), link_graph);
202  *progress = 2; // Progress bar feedback
203  boost::this_thread::interruption_point();
204 
205  // 2. DISABLE ALL ADJACENT LINK COLLISIONS ---------------------------------------------------------
206  // if 2 links are adjacent, or adjacent with a zero-shape between them, disable collision checking for them
207  unsigned int num_adjacent = disableAdjacentLinks(*scene, link_graph, link_pairs);
208  *progress = 4; // Progress bar feedback
209  boost::this_thread::interruption_point();
210 
211  // 3. INITIAL CONTACTS TO CONSIDER GUESS -----------------------------------------------------------
212  // Create collision detection request object
214  req.contacts = true;
215  // max number of contacts to compute. initial guess is number of links on robot
216  req.max_contacts = int(link_graph.size());
217  req.max_contacts_per_pair = 1;
218  req.verbose = false;
219 
220  // 4. DISABLE "DEFAULT" COLLISIONS --------------------------------------------------------
221  // Disable all collision checks that occur when the robot is started in its default state
222  unsigned int num_default = disableDefaultCollisions(*scene, link_pairs, req);
223  *progress = 6; // Progress bar feedback
224  boost::this_thread::interruption_point();
225 
226  // 5. ALWAYS IN COLLISION --------------------------------------------------------------------
227  // Compute the links that are always in collision
228  unsigned int num_always =
229  disableAlwaysInCollision(*scene, link_pairs, req, links_seen_colliding, min_collision_fraction);
230  // ROS_INFO("Links seen colliding total = %d", int(links_seen_colliding.size()));
231  *progress = 8; // Progress bar feedback
232  boost::this_thread::interruption_point();
233 
234  // 6. NEVER IN COLLISION -------------------------------------------------------------------
235  // Get the pairs of links that are never in collision
236  unsigned int num_never = 0;
237  if (include_never_colliding) // option of function
238  {
239  num_never = disableNeverInCollision(num_trials, *scene, link_pairs, req, links_seen_colliding, progress);
240  }
241 
242  // ROS_INFO("Link pairs seen colliding ever: %d", int(links_seen_colliding.size()));
243 
244  if (verbose)
245  {
246  // Calculate number of disabled links:
247  unsigned int num_disabled = 0;
248  for (LinkPairMap::const_iterator pair_it = link_pairs.begin(); pair_it != link_pairs.end(); ++pair_it)
249  {
250  if (pair_it->second.disable_check) // has a reason to be disabled
251  ++num_disabled;
252  }
253 
254  ROS_INFO("-------------------------------------------------------------------------------");
255  ROS_INFO("Statistics:");
256  unsigned int num_links = int(link_graph.size());
257  double num_possible = boost::math::binomial_coefficient<double>(num_links, 2); // n choose 2
258  unsigned int num_sometimes = num_possible - num_disabled;
259 
260  ROS_INFO("%6d : %s", num_links, "Total Links");
261  ROS_INFO("%6.0f : %s", num_possible, "Total possible collisions");
262  ROS_INFO("%6d : %s", num_always, "Always in collision");
263  ROS_INFO("%6d : %s", num_never, "Never in collision");
264  ROS_INFO("%6d : %s", num_default, "Default in collision");
265  ROS_INFO("%6d : %s", num_adjacent, "Adjacent links disabled");
266  ROS_INFO("%6d : %s", num_sometimes, "Sometimes in collision");
267  ROS_INFO("%6d : %s", num_disabled, "TOTAL DISABLED");
268 
269  /*ROS_INFO("Copy to Spreadsheet:");
270  ROS_INFO_STREAM(num_links << "\t" << num_possible << "\t" << num_always << "\t" << num_never
271  << "\t" << num_default << "\t" << num_adjacent << "\t" << num_sometimes
272  << "\t" << num_disabled);
273  */
274  }
275 
276  *progress = 100; // end the status bar
277 
278  return link_pairs;
279 }
280 
281 // ******************************************************************************************
282 // Helper function for adding two links to the disabled links data structure
283 // ******************************************************************************************
284 bool setLinkPair(const std::string& linkA, const std::string& linkB, const DisabledReason reason,
285  LinkPairMap& link_pairs)
286 {
287  bool isUnique = false; // determine if this link pair had already existsed in the link_pairs datastructure
288 
289  // Determine order of the 2 links in the pair
290  std::pair<std::string, std::string> link_pair;
291 
292  // compare the string names of the two links and add the lesser alphabetically, s.t. the pair is only added once
293  if (linkA < linkB)
294  {
295  link_pair = std::pair<std::string, std::string>(linkA, linkB);
296  }
297  else
298  {
299  link_pair = std::pair<std::string, std::string>(linkB, linkA);
300  }
301 
302  // Update properties of this link pair using only 1 search
303  LinkPairData* link_pair_ptr = &link_pairs[link_pair];
304 
305  // Check if link pair was already disabled. It also creates the entry if none existed
306  if (link_pairs[link_pair].disable_check == false) // it was not previously disabled
307  {
308  isUnique = true;
309  link_pair_ptr->reason = reason; // only change the reason if the pair was previously enabled
310  }
311 
312  // Only disable collision checking if there is a reason to disable it. This func is also used for initializing pairs
313  link_pair_ptr->disable_check = (reason != NOT_DISABLED);
314 
315  return isUnique;
316 }
317 
318 // ******************************************************************************************
319 // Generate a list of unique link pairs for all links with geometry. Order pairs alphabetically. n choose 2 pairs
320 // ******************************************************************************************
322 {
323  // Get the names of the link models that have some collision geometry associated to themselves
324  const std::vector<std::string>& names = scene.getRobotModel()->getLinkModelNamesWithCollisionGeometry();
325 
326  std::pair<std::string, std::string> temp_pair;
327 
328  // Loop through every combination of name pairs, AB and BA, n^2
329  for (std::size_t i = 0; i < names.size(); ++i)
330  {
331  for (std::size_t j = i + 1; j < names.size(); ++j)
332  {
333  // Add to link pairs list
334  setLinkPair(names[i], names[j], NOT_DISABLED, link_pairs);
335  }
336  }
337 }
338 // ******************************************************************************************
339 // Build the robot links connection graph and then check for links with no geomotry
340 // ******************************************************************************************
341 void computeConnectionGraph(const robot_model::LinkModel* start_link, LinkGraph& link_graph)
342 {
343  link_graph.clear(); // make sure the edges structure is clear
344 
345  // Recurively build adj list of link connections
346  computeConnectionGraphRec(start_link, link_graph);
347 
348  // Repeatidly check for links with no geometry and remove them, then re-check until no more removals are detected
349  bool update = true; // track if a no geometry link was found
350  while (update)
351  {
352  update = false; // default
353 
354  // Check if each edge has a shape
355  for (LinkGraph::const_iterator edge_it = link_graph.begin(); edge_it != link_graph.end(); ++edge_it)
356  {
357  if (edge_it->first->getShapes().empty()) // link in adjList "link_graph" does not have shape, remove!
358  {
359  // Temporary list for connected links
360  std::vector<const robot_model::LinkModel*> temp_list;
361 
362  // Copy link's parent and child links to temp_list
363  for (std::set<const robot_model::LinkModel*>::const_iterator adj_it = edge_it->second.begin();
364  adj_it != edge_it->second.end(); ++adj_it)
365  {
366  temp_list.push_back(*adj_it);
367  }
368 
369  // Make all preceeding and succeeding links to the no-shape link fully connected
370  // so that they don't collision check with themselves
371  for (std::size_t i = 0; i < temp_list.size(); ++i)
372  {
373  for (std::size_t j = i + 1; j < temp_list.size(); ++j)
374  {
375  // for each LinkModel in temp_list, find its location in the link_graph structure and insert the rest
376  // of the links into its unique set.
377  // if the LinkModel is not already in the set, update is set to true so that we keep searching
378  if (link_graph[temp_list[i]].insert(temp_list[j]).second)
379  update = true;
380  if (link_graph[temp_list[j]].insert(temp_list[i]).second)
381  update = true;
382  }
383  }
384  }
385  }
386  }
387  // ROS_INFO("Generated connection graph with %d links", int(link_graph.size()));
388 }
389 
390 // ******************************************************************************************
391 // Recursively build the adj list of link connections
392 // ******************************************************************************************
393 void computeConnectionGraphRec(const robot_model::LinkModel* start_link, LinkGraph& link_graph)
394 {
395  if (start_link) // check that the link is a valid pointer
396  {
397  // Loop through every link attached to start_link
398  for (std::size_t i = 0; i < start_link->getChildJointModels().size(); ++i)
399  {
400  const robot_model::LinkModel* next = start_link->getChildJointModels()[i]->getChildLinkModel();
401 
402  // Bi-directional connection
403  link_graph[next].insert(start_link);
404  link_graph[start_link].insert(next);
405 
406  // Iterate with subsequent link
407  computeConnectionGraphRec(next, link_graph);
408  }
409  }
410  else
411  {
412  ROS_ERROR("Joint exists in URDF with no link!");
413  }
414 }
415 
416 // ******************************************************************************************
417 // Disable collision checking for adjacent links, or adjacent with no geometry links between them
418 // ******************************************************************************************
419 unsigned int disableAdjacentLinks(planning_scene::PlanningScene& scene, LinkGraph& link_graph, LinkPairMap& link_pairs)
420 {
421  int num_disabled = 0;
422  for (LinkGraph::const_iterator link_graph_it = link_graph.begin(); link_graph_it != link_graph.end(); ++link_graph_it)
423  {
424  // disable all connected links to current link by looping through them
425  for (std::set<const robot_model::LinkModel*>::const_iterator adj_it = link_graph_it->second.begin();
426  adj_it != link_graph_it->second.end(); ++adj_it)
427  {
428  // ROS_INFO("Disabled %s to %s", link_graph_it->first->getName().c_str(), (*adj_it)->getName().c_str() );
429 
430  // Check if either of the links have no geometry. If so, do not add (are we sure?)
431  if (!link_graph_it->first->getShapes().empty() && !(*adj_it)->getShapes().empty()) // both links have geometry
432  {
433  num_disabled += setLinkPair(link_graph_it->first->getName(), (*adj_it)->getName(), ADJACENT, link_pairs);
434 
435  // disable link checking in the collision matrix
436  scene.getAllowedCollisionMatrixNonConst().setEntry(link_graph_it->first->getName(), (*adj_it)->getName(), true);
437  }
438  }
439  }
440  // ROS_INFO("Disabled %d adjancent link pairs from collision checking", num_disabled);
441 
442  return num_disabled;
443 }
444 
445 // ******************************************************************************************
446 // Disable all collision checks that occur when the robot is started in its default state
447 // ******************************************************************************************
450 {
451  // Setup environment
453  scene.getCurrentStateNonConst().setToDefaultValues(); // set to default values of 0 OR half between low and high
454  // joint values
455  scene.checkSelfCollision(req, res);
456 
457  // For each collision in default state, always add to disabled links set
458  int num_disabled = 0;
459  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin();
460  it != res.contacts.end(); ++it)
461  {
462  num_disabled += setLinkPair(it->first.first, it->first.second, DEFAULT, link_pairs);
463 
464  // disable link checking in the collision matrix
465  scene.getAllowedCollisionMatrixNonConst().setEntry(it->first.first, it->first.second, true);
466  }
467 
468  // ROS_INFO("Disabled %d link pairs that are in collision in default state from collision checking", num_disabled);
469 
470  return num_disabled;
471 }
472 
473 // ******************************************************************************************
474 // Compute the links that are always in collision
475 // ******************************************************************************************
477  collision_detection::CollisionRequest& req, StringPairSet& links_seen_colliding,
478  double min_collision_faction)
479 {
480  // Trial count variables
481  static const unsigned int small_trial_count = 200;
482  static const unsigned int small_trial_limit = (unsigned int)((double)small_trial_count * min_collision_faction);
483 
484  bool done = false;
485  unsigned int num_disabled = 0;
486 
487  while (!done)
488  {
489  // DO 'small_trial_count' COLLISION CHECKS AND RECORD STATISTICS ---------------------------------------
490  std::map<std::pair<std::string, std::string>, unsigned int> collision_count;
491 
492  // Do a large number of tests
493  for (unsigned int i = 0; i < small_trial_count; ++i)
494  {
495  // Check for collisions
497  scene.getCurrentStateNonConst().setToRandomPositions();
498  scene.checkSelfCollision(req, res);
499 
500  // Sum the number of collisions
501  unsigned int nc = 0;
502  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin();
503  it != res.contacts.end(); ++it)
504  {
505  collision_count[it->first]++;
506  links_seen_colliding.insert(it->first);
507  nc += it->second.size();
508  }
509 
510  // Check if the number of contacts is greater than the max count
511  if (nc >= req.max_contacts)
512  {
513  req.max_contacts *= 2; // double the max contacts that the CollisionRequest checks for
514  // ROS_INFO("Doubling max_contacts to %d", int(req.max_contacts));
515  }
516  }
517 
518  // >= XX% OF TIME IN COLLISION DISABLE -----------------------------------------------------
519  // Disable collision checking for links that are >= XX% of the time in collision (XX% = 95% by default)
520  int found = 0;
521 
522  // Loop through every pair of link collisions and disable if it meets the threshold
523  for (std::map<std::pair<std::string, std::string>, unsigned int>::const_iterator it = collision_count.begin();
524  it != collision_count.end(); ++it)
525  {
526  // Disable these two links permanently
527  if (it->second > small_trial_limit)
528  {
529  num_disabled += setLinkPair(it->first.first, it->first.second, ALWAYS, link_pairs);
530 
531  // disable link checking in the collision matrix
532  scene.getAllowedCollisionMatrixNonConst().setEntry(it->first.first, it->first.second, true);
533 
534  found++;
535  }
536  }
537 
538  // if no updates were made to the collision matrix, we stop
539  if (found == 0)
540  done = true;
541 
542  // ROS_INFO("Disabled %u link pairs that are always in collision from collision checking", found);
543  }
544 
545  return num_disabled;
546 }
547 
548 // ******************************************************************************************
549 // Get the pairs of links that are never in collision
550 // ******************************************************************************************
551 unsigned int disableNeverInCollision(const unsigned int num_trials, planning_scene::PlanningScene& scene,
552  LinkPairMap& link_pairs, const collision_detection::CollisionRequest& req,
553  StringPairSet& links_seen_colliding, unsigned int* progress)
554 {
555  unsigned int num_disabled = 0;
556 
557  boost::thread_group bgroup; // create a group of threads
558  boost::mutex lock; // used for sharing the same data structures
559 
560  int num_threads = boost::thread::hardware_concurrency(); // how many cores does this computer have?
561  // ROS_INFO_STREAM("Performing " << num_trials << " trials for 'always in collision' checking on " <<
562  // num_threads << " threads...");
563 
564  for (int i = 0; i < num_threads; ++i)
565  {
566  ThreadComputation tc(scene, req, i, num_trials / num_threads, &links_seen_colliding, &lock, progress);
567  bgroup.create_thread(boost::bind(&disableNeverInCollisionThread, tc));
568  }
569 
570  try
571  {
572  bgroup.join_all(); // wait for all threads to finish
573  }
574  catch (boost::thread_interrupted)
575  {
576  ROS_WARN("disableNeverInCollision interrupted");
577  bgroup.interrupt_all();
578  bgroup.join_all(); // wait for all threads to interrupt
579  throw;
580  }
581 
582  // Loop through every possible link pair and check if it has ever been seen in collision
583  for (LinkPairMap::iterator pair_it = link_pairs.begin(); pair_it != link_pairs.end(); ++pair_it)
584  {
585  if (!pair_it->second.disable_check) // is not disabled yet
586  {
587  // Check if current pair has been seen colliding ever. If it has never been seen colliding, add it to disabled
588  // list
589  if (links_seen_colliding.find(pair_it->first) == links_seen_colliding.end())
590  {
591  // Add to disabled list using pair ordering
592  pair_it->second.reason = NEVER;
593  pair_it->second.disable_check = true;
594 
595  // Count it
596  ++num_disabled;
597  }
598  }
599  }
600  // ROS_INFO("Disabled %d link pairs that are never in collision", num_disabled);
601 
602  return num_disabled;
603 }
604 
605 // ******************************************************************************************
606 // Thread for getting the pairs of links that are never in collision
607 // ******************************************************************************************
609 {
610  // ROS_INFO_STREAM("Thread " << tc.thread_id_ << " running " << tc.num_trials_ << " trials");
611 
612  // User feedback vars
613  const unsigned int progress_interval = tc.num_trials_ / 20; // show progress update every 5%
614 
615  // Create a new kinematic state for this thread to work on
616  robot_state::RobotState kstate(tc.scene_.getRobotModel());
617 
618  // Do a large number of tests
619  for (unsigned int i = 0; i < tc.num_trials_; ++i)
620  {
621  boost::this_thread::interruption_point();
622 
623  // Status update at intervals and only for 0 thread
624  if (i % progress_interval == 0 && tc.thread_id_ == 0)
625  {
626  (*tc.progress_) = i * 92 / tc.num_trials_ + 8; // 8 is the amount of progress already completed in prev steps
627  }
628 
630  kstate.setToRandomPositions();
631  tc.scene_.checkSelfCollision(tc.req_, res, kstate);
632 
633  // Check all contacts
634  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin();
635  it != res.contacts.end(); ++it)
636  {
637  // Check if this collision pair is unique before doing a thread lock
638  if (tc.links_seen_colliding_->find(it->first) == tc.links_seen_colliding_->end())
639  {
640  // Collision Matrix and links_seen_colliding is modified only if needed, based on above if statement
641 
642  boost::mutex::scoped_lock slock(*tc.lock_);
643  tc.links_seen_colliding_->insert(it->first);
644 
645  tc.scene_.getAllowedCollisionMatrixNonConst().setEntry(it->first.first, it->first.second,
646  true); // disable link checking in the collision matrix
647  }
648  }
649  }
650 }
651 
652 // ******************************************************************************************
653 // Converts a reason for disabling a link pair into a string
654 // ******************************************************************************************
655 const std::string disabledReasonToString(DisabledReason reason)
656 {
657  return reasonsToString.at(reason);
658 }
659 
660 // ******************************************************************************************
661 // Converts a string reason for disabling a link pair into a struct data type
662 // ******************************************************************************************
663 DisabledReason disabledReasonFromString(const std::string& reason)
664 {
666  try
667  {
668  r = reasonsFromString.at(reason);
669  }
670  catch (std::out_of_range)
671  {
672  r = USER;
673  }
674 
675  return r;
676 }
677 
678 } // namespace
const boost::unordered_map< std::string, DisabledReason > reasonsFromString
robot_state::RobotState & getCurrentStateNonConst()
Store details on a pair of links.
static unsigned int disableDefaultCollisions(planning_scene::PlanningScene &scene, LinkPairMap &link_pairs, collision_detection::CollisionRequest &req)
Disable all collision checks that occur when the robot is started in its default state.
static unsigned int disableNeverInCollision(const unsigned int num_trials, planning_scene::PlanningScene &scene, LinkPairMap &link_pairs, const collision_detection::CollisionRequest &req, StringPairSet &links_seen_colliding, unsigned int *progress)
Get the pairs of links that are never in collision.
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
static unsigned int disableAlwaysInCollision(planning_scene::PlanningScene &scene, LinkPairMap &link_pairs, collision_detection::CollisionRequest &req, StringPairSet &links_seen_colliding, double min_collision_faction=0.95)
Compute the links that are always in collision.
const collision_detection::CollisionRequest & req_
static void computeConnectionGraph(const robot_model::LinkModel *link, LinkGraph &link_graph)
Build the robot links connection graph and then check for links with no geomotry. ...
bool found
ThreadComputation(planning_scene::PlanningScene &scene, const collision_detection::CollisionRequest &req, int thread_id, int num_trials, StringPairSet *links_seen_colliding, boost::mutex *lock, unsigned int *progress)
#define ROS_WARN(...)
static bool setLinkPair(const std::string &linkA, const std::string &linkB, const DisabledReason reason, LinkPairMap &link_pairs)
Helper function for adding two links to the disabled links data structure.
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
void computeLinkPairs(const planning_scene::PlanningScene &scene, LinkPairMap &link_pairs)
Generate a list of unique link pairs for all links with geometry. Order pairs alphabetically. n choose 2 pairs.
std::map< const robot_model::LinkModel *, std::set< const robot_model::LinkModel * > > LinkGraph
const robot_model::RobotModelConstPtr & getRobotModel() const
#define ROS_INFO(...)
const std::string disabledReasonToString(DisabledReason reason)
Converts a reason for disabling a link pair into a string.
static void computeConnectionGraphRec(const robot_model::LinkModel *link, LinkGraph &link_graph)
Recursively build the adj list of link connections.
static unsigned int disableAdjacentLinks(planning_scene::PlanningScene &scene, LinkGraph &link_graph, LinkPairMap &link_pairs)
Disable collision checking for adjacent links, or adjacent with no geometry links between them...
r
collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrixNonConst()
DisabledReason
Reasons for disabling link pairs. Append "in collision" for understanding. NOT_DISABLED means the lin...
std::set< std::pair< std::string, std::string > > StringPairSet
res
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
LinkPairMap computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr &parent_scene, unsigned int *progress, const bool include_never_colliding, const unsigned int trials, const double min_collision_faction, const bool verbose)
Generate an adjacency list of links that are always and never in collision, to speed up collision det...
static void disableNeverInCollisionThread(ThreadComputation tc)
Thread for getting the pairs of links that are never in collision.
#define ROS_ERROR(...)
const boost::unordered_map< DisabledReason, std::string > reasonsToString
DisabledReason disabledReasonFromString(const std::string &reason)
Converts a string reason for disabling a link pair into a struct data type.
std::map< std::pair< std::string, std::string >, LinkPairData > LinkPairMap
LinkPairMap is an adjacency list structure containing links in string-based form. Used for disabled l...


moveit_setup_assistant
Author(s): Dave Coleman
autogenerated on Sun Oct 18 2020 13:19:28