00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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 }
00193
00194 #endif