bayesian_grasp_planner.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <boost/shared_ptr.hpp>
00036 #include <boost/foreach.hpp>
00037 
00038 #include "bayesian_grasp_planner/bayesian_grasp_planner.h"
00039 #include "bayesian_grasp_planner/object_detector.h"
00040 #include "bayesian_grasp_planner/grasp_evaluator.h"
00041 #include "bayesian_grasp_planner/grasp_generator.h"
00042 #include "bayesian_grasp_planner/bayesian_grasp_planner_tools.h"
00043 
00044 namespace bayesian_grasp_planner {
00045 
00046 void BayesianGraspPlanner::bayesianInference(std::vector<GraspWM> &grasps,
00047                                           const std::vector<object_manipulation_msgs::GraspableObject> &objects,
00048                                           const std::vector< boost::shared_ptr<ObjectDetector> > &object_detectors,
00049                                           const std::vector< boost::shared_ptr<GraspEvaluatorProb> > &prob_evaluators)
00050 {
00051 
00052   // compute the conditional probabilities for all grasps at once for each grasp evaluator
00053   ROS_INFO("computing grasp conditional probabilities");
00054   BOOST_FOREACH(const boost::shared_ptr<GraspEvaluatorProb> &prob_eval, prob_evaluators)
00055   {
00056     std::vector<double> grasp_success_probs;
00057     std::vector<double> grasp_failure_probs;
00058     
00059     // initialize the success_cond_probs and failure_cond_probs for all grasps
00060     BOOST_FOREACH(GraspWM &grasp, grasps)
00061     {
00062       grasp.success_cond_probs.resize(objects.size());
00063       grasp.failure_cond_probs.resize(objects.size());
00064     }
00065     
00066     // if the evaluations are linked to a particular object rep, compute the values for each object rep
00067     if(prob_eval->is_object_dependent())
00068     {
00069       for(size_t object_ind = 0; object_ind < objects.size(); object_ind++)
00070       {
00071         // ignore the nd-object case
00072         if(objects[object_ind].potential_models.empty()) continue;
00073         
00074         // evaluate all grasps at once
00075         prob_eval->getProbabilitiesForGraspList(grasps, objects[object_ind], grasp_success_probs, grasp_failure_probs);
00076         
00077         // ROS_INFO("evaluting grasps on object %d", (int)object_ind);
00078         // printf("grasp_success_prob: ");
00079         // pplist(grasp_success_probs);
00080         // printf("grasp_failure_prob: ");
00081         // pplist(grasp_failure_probs);
00082         
00083         // put the results in the appropriate list for this object rep
00084         for (size_t grasp_ind = 0; grasp_ind < grasps.size(); grasp_ind++)
00085         {
00086           grasps[grasp_ind].success_cond_probs[object_ind].push_back(grasp_success_probs[grasp_ind]);
00087           grasps[grasp_ind].failure_cond_probs[object_ind].push_back(grasp_failure_probs[grasp_ind]);
00088         }
00089       }
00090     }
00091     
00092     // evaluations aren't linked to any object rep, compute once and put them in the lists for all object reps
00093     else
00094     {
00095       // evaluate all grasps at once
00096       prob_eval->getProbabilitiesForGraspList(grasps, objects[objects.size()-1], grasp_success_probs, grasp_failure_probs);
00097       
00098       // printf("evaluating grasps with cluster planner");
00099       // printf("grasp_success_prob: ");
00100       // pplist(grasp_success_probs);
00101       // printf("grasp_failure_prob: ");
00102       // pplist(grasp_failure_probs);
00103       
00104       // put the results in the lists for all object reps
00105       for (size_t grasp_ind = 0; grasp_ind < grasps.size(); grasp_ind++)
00106       {
00107         for (size_t object_ind = 0; object_ind < objects.size(); object_ind++)
00108         {
00109           grasps[grasp_ind].success_cond_probs[object_ind].push_back(grasp_success_probs[grasp_ind]);
00110           grasps[grasp_ind].failure_cond_probs[object_ind].push_back(grasp_failure_probs[grasp_ind]);
00111         }
00112       }
00113     }
00114   }
00115 
00116   {
00117     for (size_t object_ind = 0; object_ind < objects.size(); object_ind++)
00118     {
00119       // Object prior 
00120       double object_prior;
00121       if (objects[object_ind].potential_models.size() > 0) 
00122       {        
00123         object_prior = 0.5 / (objects.size()-1);
00124         printf("Database object %d:\n", objects[object_ind].potential_models[0].model_id);
00125       }
00126       else 
00127       {
00128         printf("Non-database object:\n");
00129         object_prior = 0.5;
00130       }
00131       
00132       // Contributions from object detection results
00133       double detection_prob = 1.0;  //prod over d_j(P(d_j|o_i))
00134       BOOST_FOREACH(const boost::shared_ptr<ObjectDetector> &detector, object_detectors)
00135       {
00136         double obj_det_prob = detector->getProbabilityForDetection(objects[object_ind]);
00137         //printf("  obj_det_prob for object %d:%0.6f\n", (int)object_ind, obj_det_prob);
00138         detection_prob *= obj_det_prob;
00139       }
00140       printf("  total detection probability: %f\n", detection_prob);
00141     }
00142   }
00143 
00144  
00145   // perform inference to predict P(s|D,E) (prob of grasp success given all detection and grasp eval results)
00146   ROS_INFO("performing inference");
00147   for (size_t grasp_ind = 0; grasp_ind < grasps.size(); grasp_ind++)
00148   {
00149     double grasp_overall_success_prob = 0.0;
00150     double grasp_overall_failure_prob = 0.0;
00151     for (size_t object_ind = 0; object_ind < objects.size(); object_ind++)
00152     {
00153       // Object prior 
00154       double object_prior;
00155       if (objects[object_ind].potential_models.size() > 0) object_prior = 0.5 / (objects.size()-1);
00156       else object_prior = 0.5;
00157       
00158       // Contributions from object detection results
00159       double detection_prob = 1.0;  //prod over d_j(P(d_j|o_i))
00160       //printf("obj_det_probs: ");
00161       BOOST_FOREACH(const boost::shared_ptr<ObjectDetector> &detector, object_detectors)
00162       {
00163         double obj_det_prob = detector->getProbabilityForDetection(objects[object_ind]);
00164         //printf("obj_det_prob for object %d:%0.6f\n", (int)object_ind, obj_det_prob);
00165         //printf("%.6f ", obj_det_prob);
00166         detection_prob *= obj_det_prob;
00167       }
00168       //printf("\n");
00169       
00170       // Grasp prior 
00171       double success_prior = 0.5;  //P(s|o_i)
00172       double failure_prior = 0.5;  //P(f|o_i)
00173       
00174       // Contributions from grasp evaluation results (computed above)
00175       double grasp_success_prob = 1.0;  //product over e_k of P(e_k|s,o_i)/P(e_k|o_i)
00176       double grasp_failure_prob = 1.0;  //product over e_k of P(e_k|f,o_i)/P(e_k|o_i)
00177           /*
00178           if(grasp_ind == 60)
00179           {
00180                 printf("grasp_ind: %d\n", (int)grasp_ind);
00181                 printf("success cond probs for object %d: ", (int)object_ind);
00182                 pplist(grasps[grasp_ind].success_cond_probs[object_ind]);
00183                 printf("failure cond probs for object %d: ", (int)object_ind);
00184                 pplist(grasps[grasp_ind].failure_cond_probs[object_ind]);
00185                 }*/
00186       for (size_t eval_ind = 0; eval_ind < grasps[grasp_ind].success_cond_probs[object_ind].size(); eval_ind ++)
00187       {
00188         //P(e_k|o_i,s)
00189         double succ_prob = grasps[grasp_ind].success_cond_probs[object_ind][eval_ind];
00190         //P(e_k|o_i,f)
00191         double fail_prob = grasps[grasp_ind].failure_cond_probs[object_ind][eval_ind];
00192         //P(e_k|o_i)
00193         double eval_prob = succ_prob * success_prior + fail_prob * failure_prior; 
00194         
00195         grasp_success_prob *= succ_prob / eval_prob;
00196         grasp_failure_prob *= fail_prob / eval_prob;
00197         
00198       }
00199       
00200       // Multiply together to get the overall contribution for this object representation
00201       // P(s|D,E) = alpha * sum over o_i( P(o_i) * prod over d_j(P(d_j|o_i)) * P(s|o_i) * prod over e_k(P(e_k|s,o_i)/P(e_k|o_i)) )
00202       grasp_overall_success_prob += object_prior * detection_prob * success_prior * grasp_success_prob;
00203       grasp_overall_failure_prob += object_prior * detection_prob * failure_prior * grasp_failure_prob;
00204       // ROS_INFO("object %d: detection_prob: %.6f", (int)object_ind, detection_prob);
00205       // ROS_INFO("grasp_success_prob: %.3f, grasp_overall_success_prob contrib: %.6f", grasp_success_prob, object_prior * detection_prob * success_prior * grasp_success_prob);
00206       // ROS_INFO("grasp_failure_prob: %.3f, grasp_overall_failure_prob contrib: %.6f", grasp_failure_prob, object_prior * detection_prob * failure_prior * grasp_failure_prob);
00207       // printf("press enter to continue");
00208       // char blah[100];
00209       // std::cin.getline(blah, 100);
00210     }
00211     
00212     // Normalize so that P(s|D,E) + P(f|D,E) = 1 (normalization_factor is alpha)
00213     if (grasp_overall_success_prob + grasp_overall_failure_prob == 0)
00214     {
00215       ROS_WARN("sum of success and failure conditional probs was 0!");
00216       grasps[grasp_ind].grasp_.success_probability = 0.;
00217       continue;
00218     }
00219     double normalization_factor = 1.0 / (grasp_overall_success_prob + grasp_overall_failure_prob);
00220     grasps[grasp_ind].grasp_.success_probability = normalization_factor * grasp_overall_success_prob;
00221         //printf("%.3f ", grasps[grasp_ind].grasp_.success_probability);
00222     //printf("\n");
00223   }
00224 }
00225 
00226 
00227 void BayesianGraspPlanner::plan(const std::string &arm_name,
00228                                 const object_manipulation_msgs::GraspableObject &graspable_object,
00229                                 std::vector<object_manipulation_msgs::Grasp> &final_grasp_list)
00230 {
00231   //create the probability distributions for the cluster raw results
00232   boost::shared_ptr<ProbabilityDistribution> cluster_success_distribution( 
00233                                                                                   new BimodalGaussianProbabilityDistribution(.611, .285, .806) );
00234   boost::shared_ptr<ProbabilityDistribution> cluster_failure_distribution(                                 
00235                                                                                   new BimodalGaussianProbabilityDistribution(.563, .272, .935) );
00236 
00237   //create the probability distributions for graspit raw results
00238   boost::shared_ptr<ProbabilityDistribution> graspit_success_distribution( 
00239                                                  new GaussianProbabilityDistribution(.825, .125, 0.0, 1.0, false) );
00240   boost::shared_ptr<ProbabilityDistribution> graspit_failure_distribution( 
00241                                                  new GaussianProbabilityDistribution(.627, .376, 0.0, 1.0, false) );
00242 
00243   //create mutually exclusive individual objects (nd-object at the end)
00244   std::vector<object_manipulation_msgs::GraspableObject> objects;
00245   createMutuallyExclusiveObjectRepresentations(graspable_object, objects);
00246 
00247   //create object detectors
00248   std::vector< boost::shared_ptr<ObjectDetector> > object_detectors;
00249   createDatabaseObjectDetectors(objects, object_detectors);
00250 
00251   //create the overall list of grasp generators
00252   std::vector< boost::shared_ptr<GraspGenerator> > grasp_generators;
00253 
00254   //create the overall list of grasp evaluators
00255   std::vector< boost::shared_ptr<GraspEvaluatorProb> > grasp_evaluators;
00256 
00257   //generators and evaluators for graspit-based stuff
00258   //*** add options for expensive/service-based GraspIt! evaluators and perturbation evaluators
00259   {
00260     //create the graspit-based regression evaluator
00261     boost::shared_ptr<MultiplexEvaluator> graspit_multiplexer_regression_eval( new MultiplexEvaluator );
00262 
00263     //for each database object
00264     BOOST_FOREACH(const household_objects_database_msgs::DatabaseModelPose &model, graspable_object.potential_models)
00265     {
00266       //create a grasp generator with all grasps
00267       boost::shared_ptr<GraspGenerator> grasp_gen( new GraspGeneratorDatabaseRetriever(database_, model, arm_name, false) );
00268       grasp_gen->generateGrasps();
00269       
00270       //create a regression evaluator based on it
00271       boost::shared_ptr<RawGraspEvaluator> regression_eval( new RawGraspEvaluatorWithRegression(grasp_gen, true) );
00272       
00273       //put that in our overall multiplexer graspit-regression-based evaluator
00274       graspit_multiplexer_regression_eval->addEvaluator(regression_eval, model.model_id);
00275 
00276       //create a grasp generator with only cluster rep grasps
00277       boost::shared_ptr<GraspGenerator> grasp_gen_reps( new GraspGeneratorDatabaseRetriever(database_, model, arm_name, true) );
00278 
00279       //put it in our overall list of grasp generators
00280       grasp_generators.push_back( grasp_gen_reps );      
00281     }
00282 
00283     //create a probabilistic estimator based on the graspit raw evaluator
00284     boost::shared_ptr<GraspEvaluatorProb> prob_eval( new GraspEvaluatorProb( graspit_success_distribution,
00285                                                                              graspit_failure_distribution,
00286                                                                              graspit_multiplexer_regression_eval ) );
00287 
00288     //add it to our overall list of evaluators
00289     grasp_evaluators.push_back(prob_eval);
00290   }
00291 
00292   //generators and evaluators for cluster-based stuff
00293   
00294   {
00295     //create a grasp generator based on the cluster
00296     boost::shared_ptr<GraspGenerator> grasp_gen( new GraspGeneratorServiceCaller(nh_, "/plan_point_cluster_grasp", 
00297                                                                                  graspable_object, arm_name));
00298 
00299     //put it in our overall list of grasp generators
00300     grasp_generators.push_back(grasp_gen);
00301 
00302     int cluster_computer_type;
00303     nh_.param("cluster_computer_type",cluster_computer_type, 1);
00304     ROS_INFO("cluster_computer_type = %d", cluster_computer_type);
00305     
00306     boost::shared_ptr<RawGraspEvaluator> cluster_eval;
00307     switch (cluster_computer_type)
00308     {
00309     case 1: //create a service-based evaluator based on it
00310       {
00311         std::string test_cluster_grasp_srv;
00312         nh_.param("test_cluster_grasp_srv", test_cluster_grasp_srv, std::string("/evaluate_point_cluster_grasps"));  
00313         cluster_eval.reset( new RawGraspEvaluatorServiceCaller(nh_, test_cluster_grasp_srv, false) );
00314         break;
00315       }
00316     case 2: //create a regression evaluator based on it
00317       {
00318         cluster_eval.reset( new RawGraspEvaluatorWithRegression(grasp_gen, false) );
00319         break;
00320       }
00321     }
00322     
00323     //create a probabilistic estimator based on it
00324     boost::shared_ptr<GraspEvaluatorProb> prob_eval( new GraspEvaluatorProb( cluster_success_distribution,
00325                                                                              cluster_failure_distribution,
00326                                                                              cluster_eval ) );
00327     //add it to our overall list of evaluators
00328     grasp_evaluators.push_back(prob_eval);
00329   }
00330   
00331   ROS_INFO("%d grasp generators created", (int)grasp_generators.size());
00332 
00333   //list of grasps to evaluate
00334   std::vector<GraspWM> grasps;
00335   ROS_INFO("evaluating grasps");
00336 
00337   //if we're given a list of grasps to test, use those
00338   if (final_grasp_list.size() > 0)
00339   {
00340         //append metadata to the grasps
00341         appendMetadataToTestGrasps(final_grasp_list, grasps, graspable_object.reference_frame_id);             
00342   }
00343 
00344   //otherwise, compute the list of grasps (as GraspWMs)
00345   else
00346   {
00347         BOOST_FOREACH(boost::shared_ptr<GraspGenerator> &gen, grasp_generators)
00348         {
00349           ROS_INFO("getting grasps from grasp generator");
00350           gen->generateGrasps();
00351           gen->getGrasps(grasps);
00352         }
00353   }
00354 
00355   ROS_INFO("number of grasps to evaluate: %d", (int)grasps.size());
00356 
00357   //perform the inference
00358   bayesianInference(grasps, objects, object_detectors, grasp_evaluators);  
00359   
00360   /*
00361   printf("before sorting:");
00362   BOOST_FOREACH(GraspWM graspwm, grasps)
00363   {
00364         printf("%.3f ", graspwm.getGrasp().success_probability);
00365   }
00366   printf("\n\n");
00367   */
00368   //sort the grasps
00369   std::sort(grasps.rbegin(), grasps.rend());
00370 
00371   visualizeGrasps(grasps, &debug_preprune_grasp_marker_publisher_, false);
00372   /*
00373   printf("after sorting, before clustering:");
00374   BOOST_FOREACH(GraspWM graspwm, grasps)
00375   {
00376         printf("%.3f ", graspwm.getGrasp().success_probability);
00377   }
00378   printf("\n\n");
00379   */
00380   //if we're not testing a fixed list of grasps, prune grasps with too low probability of success
00381   if (final_grasp_list.size() == 0)
00382   {
00383     double grasp_probability_threshold;
00384     nh_.param("initial_probability_threshold", grasp_probability_threshold, 0.01);  
00385     pruneGraspList(grasps, grasp_probability_threshold);
00386   }
00387 
00388   visualizeGrasps(grasps, &debug_postprune_grasp_marker_publisher_, false);
00389 
00390   //cluster the grasps
00391   ROS_INFO("clustering");
00392   std::vector<GraspWM> clustered_grasps;
00393   clusterGrasps(grasps, clustered_grasps);
00394 
00395   //take at most quantity_threshold grasps
00396   int grasps_max;
00397   nh_.param("quantity_threshold", grasps_max, 100);
00398   size_t grasps_max_size = grasps_max;
00399   if (clustered_grasps.size() > grasps_max_size)
00400   {
00401         ROS_INFO("Taking top %zd grasps after clustering", grasps_max_size);
00402         clustered_grasps.resize(grasps_max_size);
00403   }    
00404 
00405   //copy the final grasps over
00406   final_grasp_list.clear();
00407   final_grasp_list.reserve(clustered_grasps.size());
00408   BOOST_FOREACH(GraspWM graspwm, clustered_grasps)
00409   {
00410         final_grasp_list.push_back(graspwm.getGrasp());
00411         //printf("%.3f ", graspwm.getGrasp().success_probability);
00412   }
00413   visualizeGrasps(clustered_grasps, &grasp_marker_publisher_, false);
00414 
00415   ROS_INFO("done planning");
00416 }
00417 
00418 void BayesianGraspPlanner::createMutuallyExclusiveObjectRepresentations(
00419                                                const object_manipulation_msgs::GraspableObject &original_object,
00420                                                std::vector<object_manipulation_msgs::GraspableObject> &me_objects )
00421 {
00422   //one for each database model/pose detected
00423   for (size_t i = 0; i < original_object.potential_models.size(); i++)
00424   {
00425         object_manipulation_msgs::GraspableObject new_object;
00426         new_object.potential_models.push_back(original_object.potential_models[i]);
00427         new_object.reference_frame_id = original_object.reference_frame_id;
00428         me_objects.push_back(new_object);
00429   }
00430 
00431   //one for the non-database object case (containing just the cluster)
00432   object_manipulation_msgs::GraspableObject new_object;
00433   new_object.cluster = original_object.cluster;
00434   new_object.reference_frame_id = original_object.reference_frame_id;
00435   me_objects.push_back(new_object);
00436 
00437   ROS_INFO("%d object representations created", (int)me_objects.size());
00438 }
00439 
00440 void BayesianGraspPlanner::createDatabaseObjectDetectors( 
00441                                     const std::vector<object_manipulation_msgs::GraspableObject> &objects,              
00442                                     std::vector< boost::shared_ptr<ObjectDetector> >&detectors)
00443 {
00444   //get params for the available object detectors from a yaml file
00445   //and initialize correct and incorrect distributions for each database object detector (put into dictionary)
00446   //***
00447   boost::shared_ptr<ProbabilityDistribution> correct_distribution(
00448                                new GaussianProbabilityDistribution(.531, .125, 0., .005, true));  
00449   boost::shared_ptr<ProbabilityDistribution> incorrect_distribution(
00450                                new GaussianProbabilityDistribution(.292, .154, 0., .005, true));
00451 
00452   //one detector for each database object 
00453   for (size_t i = 0; i < objects.size(); i++)
00454   {
00455     if (objects[i].potential_models.size() != 0)
00456     {
00457       //use the distributions for the object detector that generated this detection
00458       //***
00459       boost::shared_ptr<ObjectDetector> detector ( new DatabaseObjectDetector(objects[i], correct_distribution, 
00460                                                                               incorrect_distribution) );
00461       detectors.push_back(detector);
00462     }   
00463   }             
00464   ROS_INFO("%d object detectors created", (int)detectors.size());
00465 }
00466 
00467 void BayesianGraspPlanner::appendMetadataToTestGrasps(std::vector<object_manipulation_msgs::Grasp> &input_list,
00468                                                       std::vector<GraspWM> &output_list, const std::string frame_id)
00469 {
00470 
00471   output_list.reserve(output_list.size()+input_list.size());
00472 
00473   ROS_INFO("Got %zd grasps to test",input_list.size());
00474   BOOST_FOREACH(object_manipulation_msgs::Grasp &grasp, input_list)
00475   {
00476     GraspWM grasp_wm;
00477     grasp_wm.grasp_ = grasp;
00478     grasp_wm.grasp_.success_probability = 0.0;
00479     grasp_wm.model_id_ = -1;
00480     grasp_wm.grasp_id_ = -1;
00481 
00482     grasp_wm.object_pose_.setIdentity();
00483     grasp_wm.object_pose_.frame_id_ = frame_id;
00484     grasp_wm.object_pose_.stamp_ = ros::Time(0);
00485 
00487     tf::Pose grasp_in_base_frame;
00488     tf::poseMsgToTF(grasp.grasp_pose, grasp_in_base_frame);
00489     double wrist_to_tool_point_offset_ = 0.13;
00490     tf::Pose grasp_to_tool_point(tf::Matrix3x3::getIdentity(),tf::Vector3(wrist_to_tool_point_offset_,0,0));
00491     grasp_wm.tool_point_pose_ = tf::Stamped<tf::Pose>(
00492         grasp_in_base_frame * grasp_to_tool_point, grasp_wm.object_pose_.stamp_, frame_id);
00493 
00494     output_list.push_back(grasp_wm);
00495   }
00496   ROS_DEBUG("Created %zd grasps", output_list.size());
00497 }
00498 
00499 
00500 void BayesianGraspPlanner::clusterGrasps(std::vector<GraspWM> &input_list, 
00501                                                                                  std::vector<GraspWM> &cluster_rep_list)
00502 {
00503   ROS_INFO("Clustering %zd grasps",input_list.size());
00504 
00505   double clustering_cart_distance_threshold = 0.01;
00506   double clustering_angle_distance_threshold = 0.266;//30 degrees
00507 
00508   int clusters = 0;
00509   while (!input_list.empty())
00510   {
00511     GraspWM *repGrasp = &input_list.front();
00512     repGrasp->grasp_.cluster_rep = true;
00513     cluster_rep_list.push_back(*repGrasp);
00514     input_list.erase(input_list.begin());
00515     ++clusters;
00516 
00517     int cloud=0;
00518     std::vector<GraspWM>::iterator it=input_list.begin();
00519     while (it != input_list.end())
00520     {
00521       double cart_distance, angle_distance;
00522       cluster_rep_list.back().getDistance(*it,cart_distance,angle_distance);
00523       if (cart_distance < clustering_cart_distance_threshold && 
00524           angle_distance < clustering_angle_distance_threshold)
00525       {
00526         ++cloud;
00527         it = input_list.erase(it);
00528       }
00529       else {
00530         ++it;
00531       }
00532     }
00533     ROS_DEBUG("Grouped cluster of size %d",cloud);
00534   }
00535   ROS_INFO("Selected %d cluster rep grasps",clusters);
00536 }
00537 
00538 void BayesianGraspPlanner::pruneGraspList(std::vector<GraspWM> &grasps, const double threshold)
00539 {
00540   std::vector<GraspWM>::iterator it=grasps.begin();
00541   int erased_count = 0;
00542   while( it != grasps.end())
00543   {
00544     if (it->grasp_.success_probability < threshold)
00545     {
00546       erased_count++;
00547       ROS_DEBUG("Erasing grasp with probability %g",it->grasp_.success_probability);
00548       it=grasps.erase(it);
00549     }
00550     else{
00551       it++;
00552     }
00553   }
00554 
00555   ROS_INFO("Removed %d grasps below threshold",erased_count);
00556 }
00557 
00558 
00559 
00560 void BayesianGraspPlanner::visualizeGrasps(const std::vector<GraspWM> &grasps,
00561         object_manipulator::GraspMarkerPublisher *grasp_publisher, bool color_by_rank)
00562 {
00563   grasp_publisher->clearAllMarkers();
00564   size_t sz = grasps.size();
00565   if (sz == 0)
00566   {
00567     return;
00568   }
00569 
00570   for (size_t i=0; i < sz; i++) {
00571     const GraspWM *grasp = &grasps[i];
00572     std::string ns_append = boost::lexical_cast<std::string>(grasp->model_id_);
00573     grasp_publisher->setNamespaceSuffix(ns_append);
00574 
00575     tf::Pose grasp_pose_tf;
00576     tf::poseMsgToTF(grasp->grasp_.grasp_pose, grasp_pose_tf);
00577 
00578     tf::Stamped<tf::Pose> grasp_in_world(grasp_pose_tf, grasp->object_pose_.stamp_, grasp->object_pose_.frame_id_);
00579 
00580     geometry_msgs::PoseStamped grasp_in_world_msg;
00581     tf::poseStampedTFToMsg(grasp_in_world, grasp_in_world_msg);
00582 
00583     unsigned int id = grasp_publisher->addGraspMarker(grasp_in_world_msg);
00584 
00585     float colorval;
00586     if (color_by_rank)
00587     {
00588       colorval = i/(1.0*sz);
00589     }
00590     else {
00591       colorval = 1 - grasp->grasp_.success_probability;
00592     }
00593     grasp_publisher->colorGraspMarker(id, colorval, 1-colorval, 0.0);
00594   }
00595 }
00596 
00597 
00598 } //namespace
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


bayesian_grasp_planner
Author(s): Kaijen Hsiao and Matei Ciocarlie
autogenerated on Fri Jan 25 2013 15:02:36