$search
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 #ifndef _GRASP_SUCCESS_PROBABILITY_COMPUTER_ 00038 #define _GRASP_SUCCESS_PROBABILITY_COMPUTER_ 00039 00040 #include <vector> 00041 #include <boost/shared_ptr.hpp> 00042 using boost::shared_ptr; 00043 00044 #include <object_manipulation_msgs/GraspableObject.h> 00045 00046 #include <household_objects_database_msgs/DatabaseModelPose.h> 00047 00048 #include <object_manipulation_msgs/GraspPlanning.h> 00049 00050 #include "probabilistic_grasp_planner/probabilistic_planner_tools.h" 00051 00052 #include "probabilistic_grasp_planner/grasp_regression_evaluator.h" 00053 00054 #include "probabilistic_grasp_planner/forward_decls.h" 00055 #include "probabilistic_grasp_planner/grasp_retriever.h" 00056 00057 namespace probabilistic_grasp_planner { 00058 00060 class GraspSuccessProbabilityComputer 00061 { 00062 public: 00063 //virtual double operator()(const GraspWithMetadata &grasp) = 0; 00064 virtual double getProbability(const GraspWithMetadata &grasp) = 0; 00065 00067 virtual void getProbabilities(const std::vector<GraspWithMetadata> &grasps, 00068 std::vector<double> &probabilities) 00069 { 00070 probabilities.resize(grasps.size()); 00071 for (size_t grasp_idx=0; grasp_idx < grasps.size(); grasp_idx++) 00072 { 00073 probabilities[grasp_idx] = getProbability(grasps[grasp_idx]); 00074 } 00075 } 00076 }; 00077 typedef boost::shared_ptr<GraspSuccessProbabilityComputer> GraspSuccessProbabilityComputerPtr; 00078 00079 class GSPCWithEstimation : public GraspSuccessProbabilityComputer 00080 { 00081 protected: 00082 GraspRegressionEvaluator estimator_; 00083 public: 00084 GSPCWithEstimation(const std::vector<GraspWithMetadata> &known_grasps, 00085 boost::shared_ptr<GraspSuccessProbabilityComputer> &simple_computer, 00086 double position_bandwidth, double orientation_concentration) : 00087 estimator_(known_grasps, simple_computer, position_bandwidth, orientation_concentration) 00088 { 00089 00090 } 00091 00092 double getProbability(const GraspWithMetadata &grasp) 00093 { 00094 return estimator_.estimateProbability(grasp); 00095 } 00096 00097 }; 00098 00100 class GSPCServiceCaller : public GraspSuccessProbabilityComputer 00101 { 00102 private: 00104 std::string service_name_; 00105 00107 ServiceWrapper<object_manipulation_msgs::GraspPlanning> client_; 00108 00110 object_manipulation_msgs::GraspableObject target_; 00111 00113 ros::NodeHandle priv_nh_; 00114 00116 ros::NodeHandle root_nh_; 00117 public: 00119 GSPCServiceCaller(std::string service_name, const object_manipulation_msgs::GraspableObject &target); 00120 00122 virtual double getProbability(const GraspWithMetadata &grasp); 00123 00125 virtual void getProbabilities(const std::vector<GraspWithMetadata> &grasps, 00126 std::vector<double> &probabilities); 00127 }; 00128 00130 00134 class SimpleGraspSuccessProbabilityComputer : public GraspSuccessProbabilityComputer 00135 { 00136 private: 00138 int model_id_; 00140 double energy_threshold_; 00141 public: 00142 00143 SimpleGraspSuccessProbabilityComputer(double model_id, double energy_threshold) : 00144 model_id_(model_id), 00145 energy_threshold_(energy_threshold) {} 00146 00147 virtual double getProbability(const GraspWithMetadata &grasp); 00148 }; 00149 typedef boost::shared_ptr<SimpleGraspSuccessProbabilityComputer> SimpleGraspSuccessProbabilityComputerPtr; 00150 00151 class PositionRobustGraspSuccessProbabilityComputer : public GraspSuccessProbabilityComputer 00152 { 00153 protected: 00154 PerturbationGraspRetriever perturbation_grasp_retriever_; 00155 GraspSuccessProbabilityComputerPtr probability_computer_; 00156 shared_ptr<DistributionEvaluator> normal_evaluator_; 00157 00158 public: 00159 PositionRobustGraspSuccessProbabilityComputer(ObjectsDatabasePtr database, 00160 const household_objects_database_msgs::DatabaseModelPose &model, 00161 GraspSuccessProbabilityComputerPtr success_probability_computer, 00162 shared_ptr<DistributionEvaluator> normal_evaluator, 00163 const std::string &arm_name) : 00164 perturbation_grasp_retriever_(database,model, arm_name), 00165 probability_computer_(success_probability_computer), 00166 normal_evaluator_(normal_evaluator) 00167 { 00168 ROS_DEBUG("Created new position robust GSPC"); 00169 } 00170 00171 virtual double getProbability(const GraspWithMetadata &grasp); 00172 00173 }; 00174 typedef boost::shared_ptr<PositionRobustGraspSuccessProbabilityComputer> OverallGraspSuccessProbabilityComputerPtr; 00175 00176 class SimplePointClusterGSPC : public GraspSuccessProbabilityComputer 00177 { 00178 public: 00179 SimplePointClusterGSPC() {} 00180 virtual double getProbability(const GraspWithMetadata &grasp) 00181 { 00183 if (grasp.model_id_ == -1) 00184 { 00185 return grasp.grasp_.success_probability; 00186 } 00187 return 0.0; 00188 } 00189 }; 00190 00191 00192 } //namespace 00193 00194 #endif