collision_operations_generator.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2011, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *  \author E. Gil Jones
00036  *********************************************************************/
00037 
00038 #include <ros/ros.h>
00039 
00040 #include <planning_environment/util/collision_operations_generator.h>
00041 #include <yaml-cpp/yaml.h>
00042 
00043 using namespace planning_environment;
00044 
00045 inline double gen_rand(double min, double max)
00046 {
00047   int rand_num = rand()%100+1;
00048   double result = min + (double)((max-min)*rand_num)/101.0;
00049   return result;
00050 }
00051 
00052 CollisionOperationsGenerator::CollisionOperationsGenerator(planning_environment::CollisionModels* cm) 
00053 {
00054   setSafety(CollisionOperationsGenerator::Normal);
00055   cm_ = cm;
00056 
00057   enableAllCollisions();
00058 
00059 }
00060 
00061 void CollisionOperationsGenerator::generateAdjacentInCollisionPairs(std::vector<CollisionOperationsGenerator::StringPair>& adjacent_in_collision_pairs)
00062 {
00063   adjacent_in_collision_pairs.clear();
00064   const planning_models::KinematicModel* model = cm_->getKinematicModel();
00065   const planning_models::KinematicModel::LinkModel* link = model->getRoot()->getChildLinkModel();
00066   accumulateAdjacentLinksRecursive(link, adjacent_in_collision_pairs);
00067 }
00068 
00069 void CollisionOperationsGenerator::accumulateAdjacentLinksRecursive(const planning_models::KinematicModel::LinkModel* parent,
00070                                                                     std::vector<CollisionOperationsGenerator::StringPair>& adjacencies){
00071   std::vector<planning_models::KinematicModel::JointModel*> joints = parent->getChildJointModels();
00072   
00073   for(size_t i = 0; i < joints.size(); i++)
00074   {
00075     const planning_models::KinematicModel::JointModel* joint = joints[i];
00076     StringPair pair;
00077     pair.first = parent->getName();
00078     pair.second = joint->getChildLinkModel()->getName();
00079     adjacencies.push_back(pair);
00080     accumulateAdjacentLinksRecursive(joint->getChildLinkModel(), adjacencies);
00081   }
00082 }
00083 
00084 
00085 void CollisionOperationsGenerator::generateAlwaysInCollisionPairs(std::vector<CollisionOperationsGenerator::StringPair>& always_in_collision,
00086                                                                   std::vector<CollisionOperationsGenerator::CollidingJointValues>& in_collision_joint_values)
00087 {
00088   sampleAndCountCollisions(establish_always_num_);
00089   std::vector<double> percentages;
00090   std::map<std::string, std::map<std::string, double> > percentage_num;
00091   buildOutputStructures(establish_always_num_, 1.0, 1.0,
00092                         always_in_collision, percentages, in_collision_joint_values, percentage_num);
00093   
00094 }
00095 
00096 //assumes that always in collision pairs have been generated
00097 void CollisionOperationsGenerator::generateDefaultInCollisionPairs(std::vector<CollisionOperationsGenerator::StringPair>& default_in_collision,
00098                                                                   std::vector<CollisionOperationsGenerator::CollidingJointValues>& in_collision_joint_values)
00099 {
00100   default_in_collision.clear();
00101   in_collision_joint_values.clear();
00102 
00103   planning_models::KinematicState state(cm_->getKinematicModel());
00104   
00105   state.setKinematicStateToDefault();    
00106   CollidingJointValues cjv;
00107   state.getKinematicStateValues(cjv);
00108   std::vector<arm_navigation_msgs::ContactInformation> contacts;
00109   cm_->getAllCollisionsForState(state,
00110                                 contacts);
00111   
00112   for(unsigned int i = 0; i < contacts.size(); i++) {
00113     arm_navigation_msgs::ContactInformation& contact = contacts[i];
00114     default_in_collision.push_back(StringPair(contact.contact_body_1, contact.contact_body_2));
00115     in_collision_joint_values.push_back(cjv);
00116   }
00117 }
00118 
00119 void CollisionOperationsGenerator::generateOftenInCollisionPairs(std::vector<CollisionOperationsGenerator::StringPair>& often_in_collision,
00120                                                                  std::vector<double>& percentages, 
00121                                                                  std::vector<CollisionOperationsGenerator::CollidingJointValues>& in_collision_joint_values)
00122 {
00123   sampleAndCountCollisions(establish_often_num_);
00124   std::map<std::string, std::map<std::string, double> > percentage_num;
00125   buildOutputStructures(establish_often_num_, establish_often_percentage_, 1.0,
00126                         often_in_collision, percentages, in_collision_joint_values, percentage_num);
00127   
00128 }
00129 
00130 void CollisionOperationsGenerator::generateOccasionallyAndNeverInCollisionPairs(std::vector<CollisionOperationsGenerator::StringPair>& occasionally_in_collision_pairs,
00131                                                                                 std::vector<CollisionOperationsGenerator::StringPair>& never_in_collision_pairs,
00132                                                                                 std::vector<double>& collision_percentages,
00133                                                                                 std::vector<CollisionOperationsGenerator::CollidingJointValues>& in_collision_joint_values)
00134 {
00135   occasionally_in_collision_pairs.clear(); 
00136   collision_percentages.clear();
00137   never_in_collision_pairs.clear();
00138   in_collision_joint_values.clear();
00139 
00140   std::vector<CollisionOperationsGenerator::StringPair> first_in_collision_pairs;
00141   std::vector<CollisionOperationsGenerator::StringPair> second_in_collision_pairs;
00142 
00143   std::vector<CollisionOperationsGenerator::CollidingJointValues> first_in_collision_joint_values;
00144   std::vector<CollisionOperationsGenerator::CollidingJointValues> second_in_collision_joint_values;
00145 
00146   std::map<std::string, std::map<std::string, double> > first_percentage_num;
00147   std::map<std::string, std::map<std::string, double> > second_percentage_num;
00148 
00149   sampleAndCountCollisions(establish_occasional_num_);
00150   buildOutputStructures(establish_occasional_num_, 1.0/(establish_occasional_num_*1.0), 1.0,
00151                         first_in_collision_pairs, collision_percentages, first_in_collision_joint_values, first_percentage_num);
00152 
00153   ROS_INFO_STREAM("First in collision size " << first_in_collision_pairs.size());
00154 
00155   sampleAndCountCollisions(establish_occasional_num_);
00156   buildOutputStructures(establish_occasional_num_, 1.0/(establish_occasional_num_*1.0), 1.0,
00157                         second_in_collision_pairs, collision_percentages, second_in_collision_joint_values, second_percentage_num);
00158 
00159   ROS_INFO_STREAM("Second in collision size " << second_in_collision_pairs.size());
00160 
00161   collision_percentages.clear();
00162   unsigned int diff_pairs = 0;
00163   std::map<std::string, std::map<std::string, bool> > already_occasionally;
00164   std::map<std::string, std::map<std::string, bool> > already_never;
00165   for(std::map<std::string, std::map<std::string, double> >::iterator it = first_percentage_num.begin();
00166       it != first_percentage_num.end();
00167       it++) {
00168     for(std::map<std::string, double>::iterator it2 = it->second.begin();
00169         it2 != it->second.end();
00170         it2++) {
00171       bool first_never = (it2->second == 0);
00172       bool second_never = (second_percentage_num[it->first][it2->first] == 0);
00173       
00174       if(first_never && second_never) {
00175         if(already_never[it->first][it2->first] ||
00176            already_never[it2->first][it->first]) {
00177           continue;
00178         }
00179         already_never[it->first][it2->first] = true;
00180         already_never[it2->first][it->first] = true;
00181         never_in_collision_pairs.push_back(StringPair(it->first, it2->first));
00182       } else if(second_never) {
00183         diff_pairs++;
00184         occasionally_in_collision_pairs.push_back(StringPair(it->first, it2->first));
00185         collision_percentages.push_back(first_percentage_num[it->first][it2->first]);
00186         for(unsigned int i = 0; i < first_in_collision_pairs.size(); i++) {
00187           if((first_in_collision_pairs[i].first == it->first && first_in_collision_pairs[i].second == it2->first) ||
00188              (first_in_collision_pairs[i].first == it2->first && first_in_collision_pairs[i].second == it->first)) {
00189             in_collision_joint_values.push_back(first_in_collision_joint_values[i]);
00190           }
00191         }
00192       } else if(first_never){
00193         diff_pairs++;
00194         occasionally_in_collision_pairs.push_back(StringPair(it->first, it2->first));
00195         collision_percentages.push_back(second_percentage_num[it->first][it2->first]);
00196         for(unsigned int i = 0; i < second_in_collision_pairs.size(); i++) {
00197           if((second_in_collision_pairs[i].first == it->first && second_in_collision_pairs[i].second == it2->first) ||
00198              (second_in_collision_pairs[i].first == it2->first && second_in_collision_pairs[i].second == it->first)) {
00199             in_collision_joint_values.push_back(second_in_collision_joint_values[i]);
00200           }
00201         }
00202       } else {
00203         if(already_occasionally[it->first][it2->first] ||
00204            already_occasionally[it2->first][it->first]) {
00205           continue;
00206         }
00207         already_occasionally[it->first][it2->first] = true;
00208         already_occasionally[it2->first][it->first] = true;
00209         occasionally_in_collision_pairs.push_back(StringPair(it->first, it2->first));
00210         collision_percentages.push_back(second_percentage_num[it->first][it2->first]);
00211         for(unsigned int i = 0; i < second_in_collision_pairs.size(); i++) {
00212           if((second_in_collision_pairs[i].first == it->first && second_in_collision_pairs[i].second == it2->first) ||
00213              (second_in_collision_pairs[i].first == it2->first && second_in_collision_pairs[i].second == it->first)) {
00214             in_collision_joint_values.push_back(second_in_collision_joint_values[i]);
00215           }
00216         }
00217       }
00218     }
00219   }
00220   ROS_INFO_STREAM("Occasionally pairs num " << occasionally_in_collision_pairs.size());
00221   ROS_INFO_STREAM("Never pairs num " << never_in_collision_pairs.size());
00222   ROS_INFO_STREAM("Diff pairs num " << diff_pairs);
00223 }
00224   
00225 void CollisionOperationsGenerator::enableAllCollisions() {
00226   collision_space::EnvironmentModel::AllowedCollisionMatrix altered_acm = cm_->getCurrentAllowedCollisionMatrix();
00227   altered_acm.changeEntry(false);
00228   cm_->setAlteredAllowedCollisionMatrix(altered_acm);
00229 }
00230 
00231 
00232 void CollisionOperationsGenerator::enablePairCollisionChecking(const CollisionOperationsGenerator::StringPair& pair){
00233   collision_space::EnvironmentModel::AllowedCollisionMatrix altered_acm = cm_->getCurrentAllowedCollisionMatrix();
00234   altered_acm.changeEntry(pair.first, pair.second, false);
00235   cm_->setAlteredAllowedCollisionMatrix(altered_acm);
00236 
00237 }
00238 void CollisionOperationsGenerator::disablePairCollisionChecking(const CollisionOperationsGenerator::StringPair& pair) {
00239   collision_space::EnvironmentModel::AllowedCollisionMatrix altered_acm = cm_->getCurrentAllowedCollisionMatrix();
00240   altered_acm.changeEntry(pair.first, pair.second, true);
00241   cm_->setAlteredAllowedCollisionMatrix(altered_acm);
00242 }
00243 
00244 void CollisionOperationsGenerator::disablePairCollisionChecking(const std::vector<CollisionOperationsGenerator::StringPair>& pair_vec) {
00245   collision_space::EnvironmentModel::AllowedCollisionMatrix altered_acm = cm_->getCurrentAllowedCollisionMatrix();
00246   for(unsigned int i = 0; i < pair_vec.size(); i++) {
00247     altered_acm.changeEntry(pair_vec[i].first, pair_vec[i].second, true);
00248   }
00249   cm_->setAlteredAllowedCollisionMatrix(altered_acm);
00250 }
00251 
00252 void CollisionOperationsGenerator::generateSamplingStructures(const std::map<std::string, bool>& add_map) {
00253   joint_bounds_map_.clear();
00254   
00255   const std::vector<planning_models::KinematicModel::JointModel*>& jmv = cm_->getKinematicModel()->getJointModels();
00256   //assuming that 0th is world joint, which we don't want to include
00257   for(unsigned int i = 1; i < jmv.size(); i++) {
00258     const std::map<std::string, std::pair<double, double> >& joint_bounds = jmv[i]->getAllVariableBounds();
00259     for(std::map<std::string, std::pair<double, double> >::const_iterator it = joint_bounds.begin();
00260         it != joint_bounds.end();
00261         it++) {
00262       if(joint_bounds_map_.find(it->first) != joint_bounds_map_.end()) {
00263         ROS_WARN_STREAM("Have repeat DOF names for " << it->first);
00264         continue;
00265       }
00266       if(add_map.find(it->first) != add_map.end()) {
00267         if(!add_map.find(it->first)->second) {
00268           continue;
00269         }
00270       }
00271       if(it->second.first > it->second.second) {
00272         ROS_WARN_STREAM("Lower bound for DOF " << it->first << " is greater than upper bound " << it->second.first << " " << it->second.second);
00273       } else if(it->second.first == -DBL_MAX) {
00274         ROS_WARN_STREAM("Some non-root DOF " << it->first << " has negative inf lower bound");
00275       } 
00276       if(it->second.second == DBL_MAX) {
00277         ROS_WARN_STREAM("Some non-root DOF " << it->first << " has inf upper bound");
00278       } 
00279       joint_bounds_map_[it->first] = it->second;        
00280     }
00281   }
00282 }
00283 
00284 void CollisionOperationsGenerator::sampleAndCountCollisions(unsigned int num) {
00285   resetCountingMap();
00286   
00287   planning_models::KinematicState state(cm_->getKinematicModel());
00288   
00289   for(unsigned int i = 0; i < num; i++) {
00290     generateRandomState(state);
00291     
00292     std::vector<arm_navigation_msgs::ContactInformation> contacts;
00293     cm_->getAllCollisionsForState(state,
00294                                   contacts);
00295 
00296     if(i != 0 && i % 10000 == 0) {
00297       ROS_INFO_STREAM("On iteration " << i);
00298     }
00299     
00300     for(unsigned int i = 0; i < contacts.size(); i++) {
00301       arm_navigation_msgs::ContactInformation& contact = contacts[i];
00302       if(collision_count_map_.find(contact.contact_body_1) == collision_count_map_.end()) {
00303         ROS_WARN_STREAM("Problem - have no count for collision body " << contact.contact_body_1);
00304       }
00305       if(collision_count_map_.find(contact.contact_body_2) == collision_count_map_.end()) {
00306         ROS_WARN_STREAM("Problem - have no count for collision body " << contact.contact_body_2);
00307       }
00308       collision_count_map_[contact.contact_body_1][contact.contact_body_2]++;
00309       collision_count_map_[contact.contact_body_2][contact.contact_body_1]++;
00310       CollidingJointValues cjv;
00311       state.getKinematicStateValues(cjv);
00312       collision_joint_values_[contact.contact_body_1][contact.contact_body_2] = cjv;
00313       collision_joint_values_[contact.contact_body_2][contact.contact_body_1] = cjv;
00314     }
00315   }
00316 }
00317 
00318 void CollisionOperationsGenerator::buildOutputStructures(unsigned int num, double low_threshold, double high_threshold, 
00319                                                          std::vector<CollisionOperationsGenerator::StringPair>& meets_threshold_collision,
00320                                                          std::vector<double>& percentages,
00321                                                          std::vector<CollisionOperationsGenerator::CollidingJointValues>& joint_values,
00322                                                          std::map<std::string, std::map<std::string, double> >& percentage_num){
00323   meets_threshold_collision.clear();
00324   percentages.clear();
00325   joint_values.clear();
00326   percentage_num.clear();
00327   
00328   bool do_output = false;
00329   for(std::map<std::string, std::map<std::string, unsigned int> >::iterator it = collision_count_map_.begin();
00330       it != collision_count_map_.end();
00331       it++) {
00332     for(std::map<std::string, unsigned int>::iterator it2 = it->second.begin();
00333         it2 != it->second.end();
00334         it2++) {
00335       if(it->first == it2->first) {
00336         continue;
00337       }  
00338       //if we've already registered this pair continue
00339       if(percentage_num.find(it->first) != percentage_num.end()) {
00340         if(percentage_num.find(it->first)->second.find(it2->first) != 
00341            percentage_num.find(it->first)->second.end()) {
00342           continue;
00343         }
00344       }
00345       if(percentage_num.find(it2->first) != percentage_num.end()) {
00346         if(percentage_num.find(it2->first)->second.find(it->first) != 
00347            percentage_num.find(it2->first)->second.end()) {
00348           continue;
00349         }
00350       }
00351       double per = (it2->second*1.0)/(num*1.0);
00352       percentage_num[it->first][it2->first] = per;
00353       percentage_num[it2->first][it->first] = per;
00354       if(per >= low_threshold && per <= high_threshold) {
00355         meets_threshold_collision.push_back(StringPair(it->first, it2->first));
00356         percentages.push_back(per);
00357         joint_values.push_back(collision_joint_values_[it->first][it2->first]);
00358       }
00359       if(do_output) {
00360         ROS_INFO_STREAM("Per between " << it->first << " and " << it2->first << " is " << per  << " low " << low_threshold << " high " << high_threshold);
00361       }
00362     }
00363     do_output = false;
00364   }
00365 }
00366 
00367 void CollisionOperationsGenerator::performanceTestSavedResults(std::map<CollisionOperationsGenerator::DisableType, std::vector<CollisionOperationsGenerator::StringPair> >& disable_types) {
00368   enableAllCollisions();
00369   ros::WallTime n1 = ros::WallTime::now();
00370   sampleAndCountCollisions(performance_testing_num_);
00371   ROS_INFO_STREAM("With no collisions disabled full collision check take an average of " 
00372                   << (ros::WallTime::now()-n1).toSec()/(performance_testing_num_/1.0) << " seconds.");
00373   for(std::map<DisableType, std::vector<StringPair> >::iterator it = disable_types.begin(); 
00374       it != disable_types.end(); it++) {
00375     disablePairCollisionChecking(it->second);
00376     std::string com = "Disabling ";
00377     if(it->first == ALWAYS) {
00378       com = "Always";
00379     } else if(it->first == DEFAULT) {
00380       com = "Default";
00381     } else if(it->first == OFTEN) {
00382       com = "Often";
00383     } else if(it->first == OCCASIONALLY) {
00384       com = "Occasionally";
00385     } else if(it->first == ADJACENT){
00386       com = "Adjacent";
00387     } else {
00388       com = "Never";
00389     }
00390     com += " in collision pairs average full check time is ";
00391     n1 = ros::WallTime::now();
00392     sampleAndCountCollisions(performance_testing_num_);
00393     ROS_INFO_STREAM(com << (ros::WallTime::now()-n1).toSec()/(performance_testing_num_/1.0) << " seconds.");
00394   }
00395 }
00396  
00397 
00398 void CollisionOperationsGenerator::outputYamlStringOfSavedResults(YAML::Emitter& outy,  const std::map<CollisionOperationsGenerator::DisableType, std::vector<CollisionOperationsGenerator::StringPair> >& disable_types) {
00399   std::map<std::pair<std::string, std::string>, bool> already_output;
00400   outy << YAML::Key << "default_collision_operations";
00401   outy << YAML::Value << YAML::BeginSeq; 
00402   for(std::map<DisableType, std::vector<StringPair> >::const_iterator it = disable_types.begin(); 
00403       it != disable_types.end(); it++) {
00404     std::string com;
00405     if(it->first == ALWAYS) {
00406       com = "Always";
00407     } else if(it->first == DEFAULT) {
00408       com = "Default";
00409     } else if(it->first == OFTEN) {
00410       com = "Often";
00411     } else if(it->first == OCCASIONALLY) {
00412       com = "Occasionally";
00413     } else if(it->first == ADJACENT){
00414       com = "Adjacent";
00415     } else {
00416       com = "Never";
00417     }
00418     com += " in collision";
00419     for(unsigned int i = 0; i < it->second.size(); i++) {
00420       std::pair<std::string, std::string> p1(it->second[i].first, it->second[i].second);
00421       std::pair<std::string, std::string> p2(it->second[i].second, it->second[i].first);
00422       if(already_output.find(p1) != already_output.end() || 
00423          already_output.find(p2) != already_output.end()) {
00424         continue;
00425       }
00426       already_output[p1] = true;
00427       already_output[p2] = true;
00428       outy << YAML::BeginMap; 
00429       outy << YAML::Key << "object1" << YAML::Value << it->second[i].first;
00430       outy << YAML::Key << "object2" << YAML::Value << it->second[i].second;
00431       outy << YAML::Key << "operation" << YAML::Value << "disable";
00432       outy << YAML::Comment(com);
00433       outy << YAML::EndMap;
00434     }
00435   }
00436 }
00437 
00438 void CollisionOperationsGenerator::resetCountingMap() {
00439   const std::vector<planning_models::KinematicModel::LinkModel*>& lmv = cm_->getKinematicModel()->getLinkModelsWithCollisionGeometry();
00440   
00441   collision_count_map_.clear();
00442   
00443   std::map<std::string, unsigned int> all_link_zero;
00444   for(unsigned int i = 0; i < lmv.size(); i++) {
00445     all_link_zero[lmv[i]->getName()] = 0;
00446   }
00447   for(unsigned int i = 0; i < lmv.size(); i++) {
00448     collision_count_map_[lmv[i]->getName()] = all_link_zero;
00449   }
00450   collision_joint_values_.clear();
00451 }
00452 
00453 void CollisionOperationsGenerator::generateRandomState(planning_models::KinematicState& state) {
00454   std::map<std::string, double> values;
00455   for(std::map<std::string, std::pair<double, double> >::iterator it = joint_bounds_map_.begin();
00456       it != joint_bounds_map_.end();
00457       it++) {
00458     values[it->first] = gen_rand(it->second.first, it->second.second);
00459     //ROS_INFO_STREAM("Value for " << it->first << " is " << values[it->first] << " bounds " << 
00460     //                it->second.first << " " << it->second.second);
00461   }
00462   state.setKinematicState(values);
00463 }


planning_environment
Author(s): Ioan Sucan
autogenerated on Mon Dec 2 2013 12:34:43