compute_default_collisions.cpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Dave Coleman */

#include <moveit/setup_assistant/tools/compute_default_collisions.h>
#include <boost/math/special_functions/binomial.hpp> // for statistics at end
#include <boost/thread.hpp>
#include <tinyxml.h>
#include <boost/lexical_cast.hpp>
#include <boost/unordered_map.hpp>
#include <boost/assign.hpp>
#include <ros/console.h>

namespace moveit_setup_assistant
{

// ******************************************************************************************
// Custom Types, Enums and Structs
// ******************************************************************************************

// Boost mapping of reasons for disabling a link pair to strings
const boost::unordered_map<DisabledReason, std::string> reasonsToString = boost::assign::map_list_of
  ( NEVER, "Never" )
  ( DEFAULT, "Default" )
  ( ADJACENT, "Adjacent" )
  ( ALWAYS, "Always" )
  ( USER, "User" )
  ( NOT_DISABLED, "Not Disabled");

const boost::unordered_map< std::string, DisabledReason> reasonsFromString = boost::assign::map_list_of
  ( "Never", NEVER )
  ( "Default", DEFAULT )
  ( "Adjacent", ADJACENT )
  ( "Always", ALWAYS )
  ( "User", USER )
  ( "Not Disabled", NOT_DISABLED );


// Unique set of pairs of links in string-based form
typedef std::set<std::pair<std::string, std::string> > StringPairSet;

// Struct for passing parameters to threads, for cleaner code
struct ThreadComputation
{
  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)
    : scene_(scene),
      req_(req),
      thread_id_(thread_id),
      num_trials_(num_trials),
      links_seen_colliding_(links_seen_colliding),
      lock_(lock),
      progress_(progress)
  {
  }
  planning_scene::PlanningScene &scene_;
  const collision_detection::CollisionRequest &req_;
  int thread_id_;
  unsigned int   num_trials_;
  StringPairSet *links_seen_colliding_;
  boost::mutex  *lock_;
  unsigned int  *progress_; // only to be updated by thread 0
};

// LinkGraph defines a Link's model and a set of unique links it connects
typedef std::map<const robot_model::LinkModel*,
                 std::set<const robot_model::LinkModel*> > LinkGraph;

// ******************************************************************************************
// Static Prototypes
// ******************************************************************************************

static bool setLinkPair(const std::string &linkA, const std::string &linkB,
                        const DisabledReason reason, LinkPairMap &link_pairs);

static void computeConnectionGraph(const robot_model::LinkModel *link, LinkGraph &link_graph);

static void computeConnectionGraphRec(const robot_model::LinkModel *link, LinkGraph &link_graph);

static unsigned int disableAdjacentLinks(planning_scene::PlanningScene &scene, LinkGraph &link_graph,
                                         LinkPairMap &link_pairs);


static unsigned int disableDefaultCollisions(planning_scene::PlanningScene &scene, LinkPairMap &link_pairs,
                                             collision_detection::CollisionRequest &req);

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);

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);

static void disableNeverInCollisionThread(ThreadComputation tc);

// ******************************************************************************************
// Generates an adjacency list of links that are always and never in collision, to speed up collision detection
// ******************************************************************************************
LinkPairMap
computeDefaultCollisions(const planning_scene::PlanningSceneConstPtr &parent_scene, unsigned int * progress,
                         const bool include_never_colliding, const unsigned int num_trials, const double min_collision_fraction,
                         const bool verbose)
{
  // Create new instance of planning scene using pointer
  planning_scene::PlanningScenePtr scene = parent_scene->diff();

  // Map of disabled collisions that contains a link as a key and an ordered list of links that are connected.
  LinkPairMap link_pairs;

  // Track unique edges that have been found to be in collision in some state
  StringPairSet links_seen_colliding;

  // LinkGraph is a custom type of a map with a LinkModel as key and a set of LinkModels as second
  LinkGraph link_graph;

  //ROS_INFO_STREAM("Initial allowed Collision Matrix Size = " << scene.getAllowedCollisions().getSize() );

  // 0. GENERATE ALL POSSIBLE LINK PAIRS -------------------------------------------------------------
  // Generate a list of unique link pairs for all links with geometry. Order pairs alphabetically.
  // There should be n choose 2 pairs
  computeLinkPairs( *scene, link_pairs );
  *progress = 1;

  // 1. FIND CONNECTING LINKS ------------------------------------------------------------------------
  // For each link, compute the set of other links it connects to via a single joint (adjacent links)
  // or via a chain of joints with intermediate links with no geometry (like a socket joint)

  // Create Connection Graph
  computeConnectionGraph(scene->getRobotModel()->getRootLink(), link_graph);
  *progress = 2; // Progress bar feedback

  // 2. DISABLE ALL ADJACENT LINK COLLISIONS ---------------------------------------------------------
  // if 2 links are adjacent, or adjacent with a zero-shape between them, disable collision checking for them
  unsigned int num_adjacent = disableAdjacentLinks( *scene, link_graph, link_pairs);
  *progress = 4; // Progress bar feedback

  // 3. INITIAL CONTACTS TO CONSIDER GUESS -----------------------------------------------------------
  // Create collision detection request object
  collision_detection::CollisionRequest req;
  req.contacts = true;
  // max number of contacts to compute. initial guess is number of links on robot
  req.max_contacts = int(link_graph.size());
  req.max_contacts_per_pair = 1;
  req.verbose = false;

  // 4. DISABLE "DEFAULT" COLLISIONS --------------------------------------------------------
  // Disable all collision checks that occur when the robot is started in its default state
  unsigned int num_default = disableDefaultCollisions(*scene, link_pairs, req);
  *progress = 6; // Progress bar feedback

  // 5. ALWAYS IN COLLISION --------------------------------------------------------------------
  // Compute the links that are always in collision
  unsigned int num_always = disableAlwaysInCollision(*scene, link_pairs, req, links_seen_colliding, min_collision_fraction);
  //ROS_INFO("Links seen colliding total = %d", int(links_seen_colliding.size()));
  *progress = 8; // Progress bar feedback

  // 6. NEVER IN COLLISION -------------------------------------------------------------------
  // Get the pairs of links that are never in collision
  unsigned int num_never = 0;
  if (include_never_colliding) // option of function
  {
    num_never = disableNeverInCollision(num_trials, *scene, link_pairs, req, links_seen_colliding, progress);
  }

  //ROS_INFO("Link pairs seen colliding ever: %d", int(links_seen_colliding.size()));

  if(verbose)
  {
    // Calculate number of disabled links:
    unsigned int num_disabled = 0;
    for ( LinkPairMap::const_iterator pair_it = link_pairs.begin() ; pair_it != link_pairs.end() ; ++pair_it)
    {
      if( pair_it->second.disable_check ) // has a reason to be disabled
        ++num_disabled;
    }

    ROS_INFO("-------------------------------------------------------------------------------");
    ROS_INFO("Statistics:");
    unsigned int num_links = int(link_graph.size());
    double num_possible = boost::math::binomial_coefficient<double>(num_links, 2); // n choose 2
    unsigned int num_sometimes = num_possible - num_disabled;

    ROS_INFO("%6d : %s",   num_links, "Total Links");
    ROS_INFO("%6.0f : %s", num_possible, "Total possible collisions");
    ROS_INFO("%6d : %s",   num_always, "Always in collision");
    ROS_INFO("%6d : %s",   num_never, "Never in collision");
    ROS_INFO("%6d : %s",   num_default, "Default in collision");
    ROS_INFO("%6d : %s",   num_adjacent, "Adjacent links disabled");
    ROS_INFO("%6d : %s",   num_sometimes, "Sometimes in collision");
    ROS_INFO("%6d : %s",   num_disabled, "TOTAL DISABLED");

    /*ROS_INFO("Copy to Spreadsheet:");
    ROS_INFO_STREAM(num_links << "\t" << num_possible << "\t" << num_always << "\t" << num_never
                    << "\t" << num_default << "\t" << num_adjacent << "\t" << num_sometimes
                    << "\t" << num_disabled);
    */
  }

  *progress = 100; // end the status bar

  return link_pairs;
}

// ******************************************************************************************
// Helper function for adding two links to the disabled links data structure
// ******************************************************************************************
bool setLinkPair(const std::string &linkA, const std::string &linkB,
                 const DisabledReason reason, LinkPairMap &link_pairs)
{
  bool isUnique = false; // determine if this link pair had already existsed in the link_pairs datastructure

  // Determine order of the 2 links in the pair
  std::pair<std::string, std::string> link_pair;

  // compare the string names of the two links and add the lesser alphabetically, s.t. the pair is only added once
  if ( linkA < linkB )
  {
    link_pair = std::pair<std::string, std::string>( linkA, linkB );
  }
  else
  {
    link_pair = std::pair<std::string, std::string>( linkB, linkA );
  }

  // Update properties of this link pair using only 1 search
  LinkPairData *link_pair_ptr = &link_pairs[ link_pair ];

  // Check if link pair was already disabled. It also creates the entry if none existed
  if( link_pairs[ link_pair ].disable_check == false ) // it was not previously disabled
  {
    isUnique = true;
    link_pair_ptr->reason = reason; // only change the reason if the pair was previously enabled
  }

  // Only disable collision checking if there is a reason to disable it. This func is also used for initializing pairs
  link_pair_ptr->disable_check = (reason != NOT_DISABLED);

  return isUnique;
}

// ******************************************************************************************
// Generate a list of unique link pairs for all links with geometry. Order pairs alphabetically. n choose 2 pairs
// ******************************************************************************************
void computeLinkPairs( const planning_scene::PlanningScene &scene, LinkPairMap &link_pairs )
{
  // Get the names of the link models that have some collision geometry associated to themselves
  const std::vector<std::string> &names = scene.getRobotModel()->getLinkModelNamesWithCollisionGeometry();

  std::pair<std::string,std::string> temp_pair;

  // Loop through every combination of name pairs, AB and BA, n^2
  for (std::size_t i = 0 ; i < names.size() ; ++i)
  {
    for (std::size_t j = i+1 ; j < names.size() ; ++j)
    {
      // Add to link pairs list
      setLinkPair( names[i], names[j], NOT_DISABLED, link_pairs );
    }
  }
}
// ******************************************************************************************
// Build the robot links connection graph and then check for links with no geomotry
// ******************************************************************************************
void computeConnectionGraph(const robot_model::LinkModel *start_link, LinkGraph &link_graph)
{
  link_graph.clear(); // make sure the edges structure is clear

  // Recurively build adj list of link connections
  computeConnectionGraphRec(start_link, link_graph);

  // Repeatidly check for links with no geometry and remove them, then re-check until no more removals are detected
  bool update = true; // track if a no geometry link was found
  while (update)
  {
    update = false; // default

    // Check if each edge has a shape
    for (LinkGraph::const_iterator edge_it = link_graph.begin() ; edge_it != link_graph.end() ; ++edge_it)
    {
      if (!edge_it->first->getShape()) // link in adjList "link_graph" does not have shape, remove!
      {
        // Temporary list for connected links
        std::vector<const robot_model::LinkModel*> temp_list;

        // Copy link's parent and child links to temp_list
        for (std::set<const robot_model::LinkModel*>::const_iterator adj_it = edge_it->second.begin();
             adj_it != edge_it->second.end();
             ++adj_it)
        {
          temp_list.push_back(*adj_it);
        }

        // Make all preceeding and succeeding links to the no-shape link fully connected
        // so that they don't collision check with themselves
        for (std::size_t i = 0 ; i < temp_list.size() ; ++i)
        {
          for (std::size_t j = i + 1 ; j < temp_list.size() ; ++j)
          {
            // for each LinkModel in temp_list, find its location in the link_graph structure and insert the rest
            // of the links into its unique set.
            // if the LinkModel is not already in the set, update is set to true so that we keep searching
            if (link_graph[temp_list[i]].insert(temp_list[j]).second)
              update = true;
            if (link_graph[temp_list[j]].insert(temp_list[i]).second)
              update = true;
          }
        }
      }
    }
  }
  //ROS_INFO("Generated connection graph with %d links", int(link_graph.size()));
}


// ******************************************************************************************
// Recursively build the adj list of link connections
// ******************************************************************************************
void computeConnectionGraphRec(const robot_model::LinkModel *start_link, LinkGraph &link_graph)
{
  if (start_link) // check that the link is a valid pointer
  {
    // Loop through every link attached to start_link
    for (std::size_t i = 0 ; i < start_link->getChildJointModels().size() ; ++i)
    {
      const robot_model::LinkModel *next = start_link->getChildJointModels()[i]->getChildLinkModel();

      // Bi-directional connection
      link_graph[next].insert(start_link);
      link_graph[start_link].insert(next);

      // Iterate with subsequent link
      computeConnectionGraphRec(next, link_graph);
    }
  }
  else
  {
    ROS_ERROR("Joint exists in URDF with no link!");
  }
}

// ******************************************************************************************
// Disable collision checking for adjacent links, or adjacent with no geometry links between them
// ******************************************************************************************
unsigned int disableAdjacentLinks(planning_scene::PlanningScene &scene, LinkGraph &link_graph, LinkPairMap &link_pairs)
{
  int num_disabled = 0;
  for (LinkGraph::const_iterator link_graph_it = link_graph.begin() ; link_graph_it != link_graph.end() ; ++link_graph_it)
  {
    // disable all connected links to current link by looping through them
    for (std::set<const robot_model::LinkModel*>::const_iterator adj_it = link_graph_it->second.begin();
         adj_it != link_graph_it->second.end();
         ++adj_it)
    {
      //ROS_INFO("Disabled %s to %s", link_graph_it->first->getName().c_str(), (*adj_it)->getName().c_str() );

      // Check if either of the links have no geometry. If so, do not add (are we sure?)
      if ( link_graph_it->first->getShape() && (*adj_it)->getShape() ) // both links have geometry
      {
        num_disabled += setLinkPair( link_graph_it->first->getName(), (*adj_it)->getName(), ADJACENT, link_pairs );

        // disable link checking in the collision matrix
        scene.getAllowedCollisionMatrixNonConst().setEntry( link_graph_it->first->getName(), (*adj_it)->getName(), true);
      }

    }
  }
  //ROS_INFO("Disabled %d adjancent link pairs from collision checking", num_disabled);

  return num_disabled;
}

// ******************************************************************************************
// Disable all collision checks that occur when the robot is started in its default state
// ******************************************************************************************
unsigned int disableDefaultCollisions(planning_scene::PlanningScene &scene, LinkPairMap &link_pairs,
                                      collision_detection::CollisionRequest &req)
{
  // Setup environment
  collision_detection::CollisionResult res;
  scene.getCurrentStateNonConst().setToDefaultValues(); // set to default values of 0 OR half between low and high joint values
  scene.checkSelfCollision(req, res);

  // For each collision in default state, always add to disabled links set
  int num_disabled = 0;
  for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin() ;
       it != res.contacts.end() ; ++it)
  {
    num_disabled += setLinkPair( it->first.first, it->first.second, DEFAULT, link_pairs );

    // disable link checking in the collision matrix
    scene.getAllowedCollisionMatrixNonConst().setEntry(it->first.first, it->first.second, true);

  }

  //ROS_INFO("Disabled %d link pairs that are in collision in default state from collision checking", num_disabled);

  return num_disabled;
}

// ******************************************************************************************
// Compute the links that are always in collision
// ******************************************************************************************
unsigned int disableAlwaysInCollision(planning_scene::PlanningScene &scene, LinkPairMap &link_pairs,
                                      collision_detection::CollisionRequest &req, StringPairSet &links_seen_colliding,
                                      double min_collision_faction)
{
  // Trial count variables
  static const unsigned int small_trial_count = 200;
  static const unsigned int small_trial_limit = (unsigned int)((double)small_trial_count * min_collision_faction);

  bool done = false;
  unsigned int num_disabled = 0;

  while (!done)
  {
    // DO 'small_trial_count' COLLISION CHECKS AND RECORD STATISTICS ---------------------------------------
    std::map<std::pair<std::string, std::string>, unsigned int> collision_count;

    // Do a large number of tests
    for (unsigned int i = 0 ; i < small_trial_count ; ++i)
    {
      // Check for collisions
      collision_detection::CollisionResult res;
      scene.getCurrentStateNonConst().setToRandomValues();
      scene.checkSelfCollision(req, res);

      // Sum the number of collisions
      unsigned int nc = 0;
      for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin() ;
           it != res.contacts.end() ; ++it)
      {
        collision_count[it->first]++;
        links_seen_colliding.insert(it->first);
        nc += it->second.size();
      }

      // Check if the number of contacts is greater than the max count
      if (nc >= req.max_contacts)
      {
        req.max_contacts *= 2; // double the max contacts that the CollisionRequest checks for
        //ROS_INFO("Doubling max_contacts to %d", int(req.max_contacts));
      }
    }

    // >= XX% OF TIME IN COLLISION DISABLE -----------------------------------------------------
    // Disable collision checking for links that are >= XX% of the time in collision (XX% = 95% by default)
    int found = 0;

    // Loop through every pair of link collisions and disable if it meets the threshold
    for (std::map<std::pair<std::string, std::string>, unsigned int>::const_iterator it = collision_count.begin() ;
         it != collision_count.end() ; ++it)
    {
      // Disable these two links permanently
      if (it->second > small_trial_limit)
      {
        num_disabled += setLinkPair( it->first.first, it->first.second, ALWAYS, link_pairs );

        // disable link checking in the collision matrix
        scene.getAllowedCollisionMatrixNonConst().setEntry(it->first.first, it->first.second, true);

        found ++;
      }

    }

    // if no updates were made to the collision matrix, we stop
    if (found == 0)
      done = true;

    //ROS_INFO("Disabled %u link pairs that are always in collision from collision checking", found);
  }

  return num_disabled;
}

// ******************************************************************************************
// Get the pairs of links that are never in collision
// ******************************************************************************************
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)
{
  unsigned int num_disabled = 0;

  boost::thread_group bgroup; // create a group of threads
  boost::mutex lock; // used for sharing the same data structures

  int num_threads = boost::thread::hardware_concurrency(); // how many cores does this computer have?
  //ROS_INFO_STREAM("Performing " << num_trials << " trials for 'always in collision' checking on " <<
  //   num_threads << " threads...");

  for(int i = 0; i < num_threads; ++i)
  {
    ThreadComputation tc(scene, req, i, num_trials/num_threads, &links_seen_colliding, &lock, progress);
    bgroup.create_thread( boost::bind( &disableNeverInCollisionThread, tc ) );
  }

  bgroup.join_all(); // wait for all threads to finish

  // Loop through every possible link pair and check if it has ever been seen in collision
  for ( LinkPairMap::iterator pair_it = link_pairs.begin() ; pair_it != link_pairs.end() ; ++pair_it)
  {
    if( ! pair_it->second.disable_check ) // is not disabled yet
    {
      // Check if current pair has been seen colliding ever. If it has never been seen colliding, add it to disabled list
      if (links_seen_colliding.find( pair_it->first ) == links_seen_colliding.end())
      {
        // Add to disabled list using pair ordering
        pair_it->second.reason = NEVER;
        pair_it->second.disable_check = true;

        // Count it
        ++num_disabled;
      }
    }
  }
  //ROS_INFO("Disabled %d link pairs that are never in collision", num_disabled);

  return num_disabled;
}

// ******************************************************************************************
// Thread for getting the pairs of links that are never in collision
// ******************************************************************************************
void disableNeverInCollisionThread(ThreadComputation tc)
{
  //ROS_INFO_STREAM("Thread " << tc.thread_id_ << " running " << tc.num_trials_ << " trials");

  // User feedback vars
  const unsigned int progress_interval = tc.num_trials_ / 20; // show progress update every 5%

  // Create a new kinematic state for this thread to work on
  robot_state::RobotState kstate(tc.scene_.getRobotModel());

  // Do a large number of tests
  for (unsigned int i = 0 ; i < tc.num_trials_ ; ++i)
  {
    // Status update at intervals and only for 0 thread
    if(i % progress_interval == 0 && tc.thread_id_ == 0)
    {
      //ROS_INFO("Collision checking %d%% complete", int(i * 100 / tc.num_trials_ ));
      (*tc.progress_) = i * 92 / tc.num_trials_ + 8; // 8 is the amount of progress already completed in prev steps
    }

    collision_detection::CollisionResult res;
    kstate.setToRandomValues();
    tc.scene_.checkSelfCollision(tc.req_, res, kstate);

    // Check all contacts
    for (collision_detection::CollisionResult::ContactMap::const_iterator it = res.contacts.begin() ; it != res.contacts.end() ; ++it)
    {
      // Check if this collision pair is unique before doing a thread lock
      if (tc.links_seen_colliding_->find( it->first ) == tc.links_seen_colliding_->end())
      {
        // Collision Matrix and links_seen_colliding is modified only if needed, based on above if statement

        boost::mutex::scoped_lock slock(*tc.lock_);
        tc.links_seen_colliding_->insert(it->first);

        tc.scene_.getAllowedCollisionMatrixNonConst().setEntry(it->first.first, it->first.second, true); // disable link checking in the collision matrix
      }

    }
  }
}

// ******************************************************************************************
// Converts a reason for disabling a link pair into a string
// ******************************************************************************************
const std::string disabledReasonToString( DisabledReason reason )
{
  return reasonsToString.at( reason );
}

// ******************************************************************************************
// Converts a string reason for disabling a link pair into a struct data type
// ******************************************************************************************
DisabledReason disabledReasonFromString( const std::string& reason )
{
  DisabledReason r;
  try
  {
    r = reasonsFromString.at( reason );
  }
  catch( std::out_of_range )
  {
    r = USER;
  }

  return r;
}


} // namespace