probabilistic_grasp_planner: probabilistic_grasp_planner: probabilistic_grasp

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 // Author(s): Peter Brook
00036 
00037 #include <algorithm>
00038 #include <fstream>
00039 
00040 #include <boost/foreach.hpp>
00041 #include <boost/lambda/bind.hpp>
00042 #include <boost/lambda/lambda.hpp>
00043 
00044 #include <object_manipulation_msgs/Grasp.h>
00045 #include <object_manipulation_msgs/GraspableObject.h>
00046 #include <household_objects_database_msgs/DatabaseModelPose.h>
00047 #include <geometry_msgs/PoseStamped.h>
00048 #include <tf/transform_datatypes.h>
00049 
00050 //#define PROF_ENABLED
00051 //#include <profiling/profiling.h>
00052 
00053 #include "probabilistic_grasp_planner/probabilistic_planner_tools.h"
00054 #include "probabilistic_grasp_planner/grasp_retriever.h"
00055 #include "probabilistic_grasp_planner/grasp_success_probability_computer.h"
00056 #include "probabilistic_grasp_planner/distribution_evaluator.h"
00057 
00058 #include "probabilistic_grasp_planner/probabilistic_grasp_planner.h"
00059 
00060 // Grasp Success Probability Computers
00061 //PROF_EXTERN(PRGSPC_PROF);
00062 //PROF_EXTERN(SIMPLE_GSPC_PROF);
00063 
00064 // Algorithm subroutines
00065 //PROF_EXTERN(NORMAL_PROF);
00066 
00067 // Grasp Retrievers
00068 //PROF_EXTERN(CLUSTER_PLANNER_CREATE_PROF);
00069 //PROF_EXTERN(CLUSTER_REP_CREATE_PROF);
00070 
00071 // Planner functions
00072 //PROF_DECLARE(PLAN_PROF);
00073 //PROF_DECLARE(CREATE_GR_PROF);
00074 //PROF_DECLARE(CREATE_PC_PROF);
00075 //PROF_DECLARE(FIRST_STAGE_PROF);
00076 //PROF_DECLARE(SECOND_STAGE_PROF);
00077 
00078 namespace probabilistic_grasp_planner {
00079 
00080 void ProbabilisticGraspPlanner::plan(const std::string &arm_name,
00081                                      object_manipulation_msgs::GraspableObject &graspable_object,
00082                                      std::vector<object_manipulation_msgs::Grasp> &final_grasp_list,
00083                                      bool visualize_results, bool prune_grasps)
00084 { 
00085   //PROF_RESET_ALL;
00086   //PROF_START_TIMER(PLAN_PROF);
00087 
00088   bool enable_cluster;
00089   nh_.param("use_cluster",enable_cluster,true);
00090 
00091   //TopHitProbabilityComputer representation_probability_computer;
00092   bool db_only = !enable_cluster;
00093   CompositeProbabilityComputer representation_probability_computer(db_only);
00094   //InverseCurveRecognitionProbabilityComputer representation_probability_computer(0.01,2.198,db_only);
00095   //LearnedProbabilityComputer representation_probability_computer;
00096 
00097   std::vector<ObjectRepresentation> representations;
00098   ROS_INFO("Creating object representations");
00099   populateRepresentationsList(representations, arm_name, graspable_object, enable_cluster);
00100   ROS_INFO("Computing representation probabilities");
00101   representation_probability_computer.computeRepresentationProbabilities(representations);
00102 
00103   printRepresentations(representations);
00104 
00105   //PROF_PRINT(CREATE_GR_PROF);
00106   //PROF_PRINT(CLUSTER_PLANNER_CREATE_PROF);
00107   //PROF_PRINT(CLUSTER_REP_CREATE_PROF);
00108   //PROF_PRINT(CREATE_PC_PROF);
00109 
00110   // generate a list of grasps g_stars
00112   std::vector<GraspWithMetadata> grasps;
00113 
00114   // If we were given grasps, we should analyze those instead of getting new grasps
00115   if (final_grasp_list.size() > 0)
00116   {
00117     ROS_INFO("Received grasps to test, using those rather than building own list");
00118     //Convert Grasp to GraspWithData
00119     appendMetadataToTestGrasps(final_grasp_list, grasps, graspable_object);
00120   }
00121   else 
00122   {
00123     BOOST_FOREACH(ObjectRepresentation &representation, representations)
00124     {
00125       if (representation.probability > 0.001)
00126       {
00127         representation.grasp_retriever->getGrasps(grasps);
00128       }
00129     }
00130   }
00131 
00132   // for each g_star evaluate probability of success
00133   double recognition_probability(0.0);
00134   double success_probability(0.0);
00135   //PROF_START_TIMER(FIRST_STAGE_PROF);
00136   ROS_INFO("Total grasps %zd",grasps.size());
00137   for (size_t representation_idx=0; representation_idx < representations.size(); representation_idx++)
00138   {
00139     recognition_probability = representations[representation_idx].probability;
00140         std::vector<double> success_probabilities;
00141         ROS_INFO("Analyzing representation %zd", representation_idx);
00142         representations[representation_idx].grasp_success_computer->getProbabilities(grasps, success_probabilities);
00143 
00144     for (size_t grasp_idx=0; grasp_idx < grasps.size(); grasp_idx++)
00145     {
00146           success_probability = success_probabilities[grasp_idx];
00147       grasps[grasp_idx].debug_probabilities.push_back
00148         (std::make_pair(representation_idx, std::make_pair(recognition_probability, success_probability)));
00149       grasps[grasp_idx].success_probability += recognition_probability*success_probability;
00150     }
00151   }
00152 
00153   //PROF_STOP_TIMER(FIRST_STAGE_PROF);
00154   //PROF_PRINT(FIRST_STAGE_PROF);
00155 
00156   // copy probability into grasp message. We can't store in there originally since 
00157   // cluster planner stores estimate there
00158   BOOST_FOREACH(GraspWithMetadata &grasp, grasps)
00159   {
00160     grasp.grasp_.success_probability = grasp.success_probability;
00161     grasp.success_probability = 0.0;
00162   }
00163 
00164   //PROF_PRINT(NORMAL_PROF);
00165   //PROF_PRINT(PRGSPC_PROF);
00166   //PROF_PRINT(SIMPLE_GSPC_PROF);
00167   // rank grasps g_star by probability and return sorted list
00168   std::sort(grasps.rbegin(), grasps.rend());
00169 
00170   ROS_INFO("First round planning returned %zd grasps.", grasps.size());
00171   if (!grasps.empty()) ROS_INFO("Top probability: %f", grasps[0].grasp_.success_probability);
00172   visualizeGrasps(grasps, &debug_preprune_grasp_marker_publisher_, false);
00173 
00174   if (prune_grasps)
00175   {
00176     //prune grasps below quality threshold
00177     double grasp_probability_threshold;
00178     nh_.param("initial_probability_threshold", grasp_probability_threshold, 0.3);  
00179     pruneGraspList(grasps, grasp_probability_threshold);
00180     ROS_INFO("  Pruned to %zd grasps above threshold of %f", grasps.size(), grasp_probability_threshold);
00181     visualizeGrasps(grasps, &debug_precluster_grasp_marker_publisher_, false);
00182 
00183     //cluster remaining grasps
00184     std::vector<GraspWithMetadata> clustered_grasps;
00185     clusterGrasps(grasps, clustered_grasps);
00186     grasps = clustered_grasps;
00187     ROS_INFO("  Clustered to %zd grasps", grasps.size());
00188 
00189     int grasps_max;
00190     nh_.param("quantity_threshold", grasps_max, 100);
00191     size_t grasps_max_size = grasps_max;
00192     if (grasps.size() > grasps_max_size)
00193     {
00194       ROS_INFO("Taking top %zd grasps after clustering", grasps_max_size);
00195       grasps.resize(grasps_max_size);
00196     }    
00197     visualizeGrasps(grasps, &debug_postprune_grasp_marker_publisher_, false);
00198   }
00199 
00200   bool second_pass;
00201   nh_.param("two_pass",second_pass,false);
00202   if (second_pass)
00203   {
00204     //PROF_START_TIMER(SECOND_STAGE_PROF);
00205     ROS_INFO("Reevaluating %zd grasps using expensive computations", grasps.size());
00206 
00207     int counter = 0;
00208     BOOST_FOREACH(ObjectRepresentation &representation, representations)
00209     {
00210       ROS_DEBUG("  Evaluating representation %d",counter);
00211       recognition_probability = representation.probability;
00213       BOOST_FOREACH(GraspWithMetadata &grasp, grasps)
00214       {
00215         //recompute the probability using the expensive method.
00216         success_probability = representation.precise_grasp_success_computer->getProbability(grasp);
00217         grasp.success_probability += recognition_probability*success_probability;
00218       }
00219       counter++;
00220     }
00221 
00222     BOOST_FOREACH(GraspWithMetadata &grasp, grasps)
00223     {
00224       grasp.grasp_.success_probability = grasp.success_probability;
00225     }
00226 
00227     std::sort(grasps.rbegin(), grasps.rend());
00228     ROS_INFO("Second pass done. Top probability: %f", grasps[0].success_probability);
00229     double final_grasp_probability_threshold;
00230     nh_.param("final_probability_threshold",final_grasp_probability_threshold,0.5);
00231     pruneGraspList(grasps, final_grasp_probability_threshold);
00232     ROS_INFO("Pruned to %zd grasps above threshold %f", grasps.size(), final_grasp_probability_threshold);
00233     //PROF_STOP_TIMER(SECOND_STAGE_PROF);
00234     //PROF_PRINT(SECOND_STAGE_PROF);
00235   }
00236 
00237   if (visualize_results)
00238   {
00239     if (debug_grasp_marker_publisher_ != NULL) visualizeGrasps(grasps, debug_grasp_marker_publisher_, false);
00240     if (rank_grasp_marker_publisher_ != NULL) visualizeGrasps(grasps, rank_grasp_marker_publisher_, true);
00241   }
00242 
00243   visualizeGrasps(grasps, grasp_marker_publisher_, false);
00244 
00245   //Need to clear it in case this is a test of the planner and there already were grasps here
00246   final_grasp_list.clear();
00247   final_grasp_list.reserve(grasps.size());
00248   BOOST_FOREACH(GraspWithMetadata &g, grasps)
00249   {
00250     final_grasp_list.push_back(g.grasp_);
00251   }
00252   //PROF_STOP_TIMER(PLAN_PROF);
00253   //PROF_PRINT(PLAN_PROF);
00254 }
00255 
00256 void ProbabilisticGraspPlanner::pruneGraspList(std::vector<GraspWithMetadata> &grasps, const double threshold)
00257 {
00258   std::vector<GraspWithMetadata>::iterator it=grasps.begin();
00259   int erased_count = 0;
00260   while( it != grasps.end())
00261   {
00262     if (it->grasp_.success_probability < threshold)
00263     {
00264       erased_count++;
00265       //ROS_DEBUG("Erasing grasp with probability %g",it->grasp_.success_probability);
00266       it=grasps.erase(it);
00267     }
00268     else{
00269       it++;
00270     }
00271   }
00272 
00273   ROS_INFO("Removed %d grasps below threshold",erased_count);
00274 }
00275 
00276 void ProbabilisticGraspPlanner::populateRepresentationsList(std::vector<ObjectRepresentation> &representations,
00277                                                             std::string arm_name,
00278                                                 const object_manipulation_msgs::GraspableObject &request_object,
00279                                                 const bool enable_cluster)
00280 {
00281   // First create an object representation for each of the potential fit models.
00282   BOOST_FOREACH(const household_objects_database_msgs::DatabaseModelPose &model_with_score,
00283                 request_object.potential_models)
00284   {
00285     representations.push_back( getObjectRepresentationFromDatabaseObject(model_with_score, arm_name) );
00286   }
00287 
00288   // Then add an object for the cluster
00289   if (enable_cluster && request_object.cluster.points.size() > 0)
00290   {
00291     representations.push_back(getObjectRepresentationFromCluster(request_object, arm_name));
00292   }
00293 }
00294 
00295 ObjectRepresentation ProbabilisticGraspPlanner::getObjectRepresentationFromCluster(
00296                                                        const object_manipulation_msgs::GraspableObject &request_object,
00297                                                        std::string arm_name)
00298 {
00299   //create the representation
00300   ObjectRepresentation representation;
00301 
00302   //create the grasp retriever for it  
00303   const std::string cluster_planner_service_name = "/plan_point_cluster_grasp";
00304   representation.grasp_retriever.reset(new ClusterPlannerGraspRetriever(nh_, cluster_planner_service_name, 
00305                                                                         request_object, arm_name));
00306 
00307   //create a copy of the graspable object with a single instance of the database model
00308   object_manipulation_msgs::GraspableObject request_object_single_instance;
00309   request_object_single_instance.cluster = request_object.cluster;
00310   representation.object = request_object_single_instance;
00311 
00312   //create a regular grasp probability computer
00313   int cluster_computer_type;
00314   nh_.param("cluster_computer_type", cluster_computer_type, 1);
00315   std::string cluster_evaluation_service_name;
00316   nh_.param("test_cluster_grasp_srv", cluster_evaluation_service_name, std::string("/evaluate_point_cluster_grasps"));
00317   switch (cluster_computer_type)
00318   {
00319   case 0: // Simple computer
00320     {
00321       ROS_DEBUG("Creating Simple Cluster Computer");
00322       representation.grasp_success_computer.reset(new SimplePointClusterGSPC);
00323       break;
00324     }
00325   case 1: // Cluster planner computer
00326     {
00327       ROS_DEBUG("Creating service caller GSPC");
00328       representation.grasp_success_computer.reset(new GSPCServiceCaller(cluster_evaluation_service_name,
00329                                                                         request_object_single_instance));
00330       break;
00331     }
00332   case 2: // Regression computer
00333     {
00334       ROS_DEBUG("Creating Regression Cluster Computer");
00335       shared_ptr<GraspSuccessProbabilityComputer> simple_cluster_computer(new SimplePointClusterGSPC);
00336       std::vector<GraspWithMetadata> grasps;
00337       representation.grasp_retriever->getGrasps(grasps);
00338       double cluster_position_sigma;
00339       double cluster_orientation_concentration;
00340       nh_.param("cluster_regression_position_sigma", cluster_position_sigma, 0.01);
00341       nh_.param("cluster_regression_orientation_concentration", cluster_orientation_concentration, 3.0);      
00342       representation.grasp_success_computer.reset(new GSPCWithEstimation(grasps, simple_cluster_computer,
00343                                                                          cluster_position_sigma,
00344                                                                          cluster_orientation_concentration));
00345       ROS_DEBUG("Regression Params %g %g",cluster_position_sigma, cluster_orientation_concentration);
00346       break;
00347     }
00348   }
00349   //create a precise grasp probability computer
00350   representation.precise_grasp_success_computer.reset(new GSPCServiceCaller(cluster_evaluation_service_name,
00351                                                                             request_object_single_instance));
00352   //and return it
00353   return representation;
00354 }
00355 
00356 ObjectRepresentation ProbabilisticGraspPlanner::getObjectRepresentationFromDatabaseObject(
00357                                             const household_objects_database_msgs::DatabaseModelPose &model_with_score,
00358                                             std::string arm_name)
00359 {
00360   //create the representation
00361   ObjectRepresentation representation;
00362 
00363   //create the grasp retriever for it
00364   shared_ptr<DatabaseGraspRetriever> retriever(new DatabaseGraspRetriever(database_, model_with_score,arm_name,true,true));
00365   ROS_INFO("Fetching grasps for model id %d", model_with_score.model_id);
00366   retriever->fetchFromDB();
00367   representation.grasp_retriever = retriever;
00368 
00369   double energy_threshold;
00370   nh_.param("db_max_energy_threshold",energy_threshold, 100.0);
00371 
00372   std::string graspit_computer_service_name, graspit_computer_robust_service_name;
00373   nh_.param<std::string>("graspit_computer_service_name", graspit_computer_service_name, 
00374                          "default_graspit_computer_service_name");
00375   nh_.param<std::string>("graspit_computer_robust_service_name", graspit_computer_robust_service_name, 
00376                          "default_graspit_computer_robust_service_name");
00377 
00378   //create a copy of the graspable object with a single instance of the database model
00379   object_manipulation_msgs::GraspableObject request_object_single_instance;
00380   request_object_single_instance.potential_models.push_back(model_with_score); 
00381   representation.object = request_object_single_instance;
00382 
00383   //create the regular grasp probability computer for it
00384   int db_computer_type;
00385   nh_.param("db_computer_type",db_computer_type,2);
00386   switch (db_computer_type)
00387   {
00388   case 0: //Simple computer
00389     {
00390       ROS_DEBUG("Creating Simple DB Computer");
00391       representation.grasp_success_computer.reset(new SimpleGraspSuccessProbabilityComputer(model_with_score.model_id, 
00392                                                                                             energy_threshold));
00393       break;
00394     }
00395   case 1: // GraspIt computer
00396     {
00397       ROS_DEBUG("Creating GraspIt DB Computer");
00398       representation.grasp_success_computer.reset( new GSPCServiceCaller(graspit_computer_service_name, 
00399                                                                          request_object_single_instance) );
00400       break;
00401     }
00402   case 2: // Regression computer
00403     {
00404           //create a separate grasp retriever to get all the database grasps
00405           shared_ptr<DatabaseGraspRetriever> all_grasps_retriever(new DatabaseGraspRetriever(database_, model_with_score,arm_name,true,false));
00406           all_grasps_retriever->fetchFromDB();
00407 
00408       ROS_DEBUG("Creating Regression DB Computer");
00409       shared_ptr<GraspSuccessProbabilityComputer> simple_probability_computer(new
00410                 SimpleGraspSuccessProbabilityComputer(model_with_score.model_id, energy_threshold));
00411       std::vector<GraspWithMetadata> grasps;
00412       all_grasps_retriever->getGrasps(grasps);
00413       double db_position_sigma, db_orientation_concentration;
00414       nh_.param("db_regression_position_sigma", db_position_sigma, 0.01);
00415       nh_.param("db_regression_orientation_concentration", db_orientation_concentration, 10.0);      
00416       representation.grasp_success_computer.reset(new GSPCWithEstimation(grasps, simple_probability_computer,
00417                                                                  db_position_sigma, db_orientation_concentration));
00418       ROS_DEBUG("Regression Params %g %g",db_position_sigma, db_orientation_concentration);
00419       break;
00420     }
00421   case 3: // Position robust computer
00422     {
00423       shared_ptr<NormalDistributionEvaluator> distribution_evaluator(new NormalDistributionEvaluator(0.01, 0.174));
00424       shared_ptr<GraspSuccessProbabilityComputer> simple_probability_computer
00425         (new SimpleGraspSuccessProbabilityComputer( model_with_score.model_id,  energy_threshold));
00426       representation.grasp_success_computer.reset
00427         (new PositionRobustGraspSuccessProbabilityComputer( database_, model_with_score, simple_probability_computer,
00428                                                             distribution_evaluator, arm_name));
00429       break;
00430     }
00431   case 4: // GraspIt robust computer
00432     {
00433       ROS_DEBUG("Creating Robust GraspIt DB Computer");
00434       representation.grasp_success_computer.reset( new GSPCServiceCaller(graspit_computer_robust_service_name, 
00435                                                                          request_object_single_instance) );
00436       break;
00437     }
00438   }
00439   //create the precise computer for it
00440   representation.precise_grasp_success_computer.reset(new GSPCServiceCaller(graspit_computer_robust_service_name, 
00441                                                                             request_object_single_instance));
00442   //and finally return it
00443   return representation;
00444 }
00445 
00446 void ProbabilisticGraspPlanner::visualizeGrasps(const std::vector<GraspWithMetadata> &grasps,
00447         object_manipulator::GraspMarkerPublisher *grasp_publisher, bool color_by_rank)
00448 {
00449   grasp_publisher->clearAllMarkers();
00450   size_t sz = grasps.size();
00451   if (sz == 0)
00452   {
00453     return;
00454   }
00455   double best_score = grasps[0].grasp_.success_probability;
00456   for (size_t i=0; i < sz; i++) {
00457     const GraspWithMetadata *grasp = &grasps[i];
00458     std::string ns_append = boost::lexical_cast<std::string>(grasp->model_id_);
00459     grasp_publisher->setNamespaceSuffix(ns_append);
00460 
00461     //tf::Stamped<tf::Pose> model_pose_tf(grasp.object_pose_);
00462 
00463     tf::Pose grasp_pose_tf;
00464     tf::poseMsgToTF(grasp->grasp_.grasp_pose, grasp_pose_tf);
00465 
00466     tf::Stamped<tf::Pose> grasp_in_world(grasp_pose_tf, grasp->object_pose_.stamp_, grasp->object_pose_.frame_id_);
00467 
00468     geometry_msgs::PoseStamped grasp_in_world_msg;
00469     tf::poseStampedTFToMsg(grasp_in_world, grasp_in_world_msg);
00470 
00471     unsigned int id = grasp_publisher->addGraspMarker(grasp_in_world_msg);
00472 
00473     float colorval;
00474     if (color_by_rank)
00475     {
00476       colorval = i/(1.0*sz);
00477     }
00478     else {
00479       colorval = 1.0 - grasp->grasp_.success_probability / (1.0*best_score);
00480     }
00481     grasp_publisher->colorGraspMarker(id, colorval, 1-colorval, 0.0);
00482   }
00483 }
00484 
00485 void ProbabilisticGraspPlanner::printRepresentations(const std::vector<ObjectRepresentation> &representations)
00486 {  
00487   ROS_INFO("Object representations:");
00488   BOOST_FOREACH(const ObjectRepresentation &representation, representations)
00489   {
00490     ROS_INFO("  Probability: %f", representation.probability);
00491     if (!representation.object.potential_models.empty())
00492     {
00493       ROS_INFO("    Database object with id %d", representation.object.potential_models[0].model_id);
00494     }
00495     else
00496     {
00497       ROS_INFO("    Cluster with %zu points", representation.object.cluster.points.size());
00498     }
00499   } 
00500 }
00501 
00502 void ProbabilisticGraspPlanner::printGrasps(const std::vector<GraspWithMetadata> &grasps)
00503 {
00504   BOOST_FOREACH(const GraspWithMetadata &grasp, grasps)
00505   {
00506     ROS_DEBUG_STREAM("Probability: "<< grasp.grasp_.success_probability << " " << grasp.grasp_id_ << " Loc " <<
00507                     grasp.grasp_.grasp_pose.position.x << " " <<
00508                     grasp.grasp_.grasp_pose.position.y << " " <<
00509                     grasp.grasp_.grasp_pose.position.z << " " <<
00510                     grasp.grasp_.grasp_pose.orientation.x << " " <<
00511                     grasp.grasp_.grasp_pose.orientation.y << " " <<
00512                     grasp.grasp_.grasp_pose.orientation.z << " " <<
00513                     grasp.grasp_.grasp_pose.orientation.w);
00514     ROS_DEBUG_STREAM("Contributions:");
00515     for (size_t i=0; i < grasp.debug_probabilities.size(); ++i)
00516     {
00517       ROS_DEBUG("%d: %g %g",grasp.debug_probabilities[i].first, grasp.debug_probabilities[i].second.first,
00518                grasp.debug_probabilities[i].second.second);
00519     }
00520   }
00521 }
00522 
00523 void 
00524 ProbabilisticGraspPlanner::appendMetadataToTestGrasps(std::vector<object_manipulation_msgs::Grasp> &input_list,
00525                                                       std::vector<GraspWithMetadata> &output_list,
00526                                                       const object_manipulation_msgs::GraspableObject &graspable_object)
00527 {
00528 
00529   output_list.reserve(output_list.size()+input_list.size());
00530 
00531   ROS_INFO("Got %zd grasps to test",input_list.size());
00532   BOOST_FOREACH(object_manipulation_msgs::Grasp &grasp, input_list)
00533   {
00534     GraspWithMetadata grasp_with_metadata;
00535     grasp_with_metadata.grasp_ = grasp;
00536     //grasp_with_metadata.grasp_.success_probability = 0.0;
00537     grasp_with_metadata.model_id_ = -1;
00538     grasp_with_metadata.grasp_id_ = -1;
00539 
00540     grasp_with_metadata.object_pose_.setIdentity();
00541     grasp_with_metadata.object_pose_.frame_id_ = graspable_object.reference_frame_id;
00542     grasp_with_metadata.object_pose_.stamp_ = ros::Time(0);
00543 
00544     tf::Pose grasp_in_base_frame;
00545     tf::poseMsgToTF(grasp.grasp_pose, grasp_in_base_frame);
00546     double wrist_to_tool_point_offset_ = 0.13;
00547     tf::Pose grasp_to_tool_point(btMatrix3x3::getIdentity(),btVector3(wrist_to_tool_point_offset_,0,0));
00549     grasp_with_metadata.tool_point_pose_ = tf::Stamped<tf::Pose>(
00550         grasp_in_base_frame * grasp_to_tool_point, grasp_with_metadata.object_pose_.stamp_,
00551         grasp_with_metadata.object_pose_.frame_id_);
00552 
00553     output_list.push_back(grasp_with_metadata);
00554   }
00555   ROS_DEBUG("Created %zd grasps",output_list.size());
00556 
00557 }
00558 
00559 void ProbabilisticGraspPlanner::clusterGrasps(std::vector<GraspWithMetadata> &input_list, 
00560                                               std::vector<GraspWithMetadata> &cluster_rep_list)
00561 {
00562   ROS_INFO("Clustering %zd grasps",input_list.size());
00563 
00564   double clustering_cart_distance_threshold = 0.01;
00565   double clustering_angle_distance_threshold = 0.266;//30 degrees
00566 
00567   int clusters = 0;
00568   while (!input_list.empty())
00569   {
00570     GraspWithMetadata *repGrasp = &input_list.front();
00571     repGrasp->grasp_.cluster_rep = true;
00572     cluster_rep_list.push_back(*repGrasp);
00573     input_list.erase(input_list.begin());
00574     ++clusters;
00575 
00576     int cloud=0;
00577     std::vector<GraspWithMetadata>::iterator it=input_list.begin();
00578     while (it != input_list.end())
00579     {
00580       double cart_distance, angle_distance;
00581       cluster_rep_list.back().getDistance(*it,cart_distance,angle_distance);
00582       if (cart_distance < clustering_cart_distance_threshold && 
00583           angle_distance < clustering_angle_distance_threshold)
00584       {
00585         ++cloud;
00590         //cluster_rep_list.push_back(*it);
00591         it = input_list.erase(it);
00592       }
00593       else {
00594         ++it;
00595       }
00596     }
00597     ROS_DEBUG("Grouped cluster of size %d",cloud);
00598   }
00599   ROS_INFO("Selected %d cluster rep grasps",clusters);
00600 }
00601 
00602 } // namespace
probabilistic_grasp_planner.cpp
