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 #include <boost/foreach.hpp>
00038
00039
00040
00041
00042 #include "probabilistic_grasp_planner/distribution_evaluator.h"
00043 #include "probabilistic_grasp_planner/grasp_retriever.h"
00044 #include "probabilistic_grasp_planner/grasp_success_probability_computer.h"
00045
00046
00047
00048
00049 namespace probabilistic_grasp_planner {
00050
00051
00052 double SimpleGraspSuccessProbabilityComputer::getProbability(const GraspWithMetadata &grasp)
00053 {
00054
00055
00056 if (model_id_ != grasp.model_id_) return 0.0;
00057
00058 double sigma_sq = 2025.0;
00059 double prob = 0.95 * std::exp(-0.5 * std::pow(grasp.energy_function_score_,2)/sigma_sq);
00060 return prob;
00061 }
00062
00063 GSPCServiceCaller::GSPCServiceCaller(std::string service_name,
00064 const object_manipulation_msgs::GraspableObject &target) :
00065 service_name_(service_name),
00066 client_(service_name),
00067 priv_nh_("~"),
00068 root_nh_("")
00069 {
00070 target_ = target;
00071 }
00072
00073 double GSPCServiceCaller::getProbability(const GraspWithMetadata &grasp)
00074 {
00075 object_manipulation_msgs::GraspPlanning plan;
00076 plan.request.target = target_;
00077 plan.request.grasps_to_evaluate.push_back(grasp.grasp_);
00078 if (!client_.client().call(plan))
00079 {
00080 ROS_ERROR("Grasp success probability computer, failed to call service at %s", service_name_.c_str());
00081 return 0.0;
00082 }
00083 ROS_ASSERT(plan.response.grasps.size() == 1);
00084 return plan.response.grasps[0].success_probability;
00085 }
00086
00087 void GSPCServiceCaller::getProbabilities(const std::vector<GraspWithMetadata> &grasps,
00088 std::vector<double> &probabilities)
00089 {
00090 probabilities.clear();
00091 probabilities.resize( grasps.size(), 0.0 );
00092 object_manipulation_msgs::GraspPlanning plan;
00093 plan.request.target = target_;
00094 BOOST_FOREACH(const GraspWithMetadata &grasp, grasps)
00095 {
00096 plan.request.grasps_to_evaluate.push_back(grasp.grasp_);
00097 }
00098 ROS_INFO("calling service to evaluate grasps");
00099 if (!client_.client().call(plan))
00100 {
00101 ROS_ERROR("Grasp success probability computer, failed to call service at %s", service_name_.c_str());
00102 return;
00103 }
00104 ROS_ASSERT(plan.response.grasps.size() == grasps.size());
00105 for (size_t i=0; i<plan.response.grasps.size(); i++)
00106 {
00107 probabilities[i] = plan.response.grasps[i].success_probability;
00108 }
00109 }
00110
00111 double PositionRobustGraspSuccessProbabilityComputer::getProbability(const GraspWithMetadata &grasp)
00112 {
00113
00114 std::vector<GraspWithMetadata> grasps;
00116 perturbation_grasp_retriever_.setGrasp(&grasp);
00118 perturbation_grasp_retriever_.getGrasps(grasps);
00119
00120 double probability = 0.0;
00121 normal_evaluator_->reset_normalization_term();
00122 BOOST_FOREACH(GraspWithMetadata &g_n, grasps)
00123 {
00124
00125 double success_probability = probability_computer_->getProbability(g_n);
00126
00127 double probability_of_grasp_execution = normal_evaluator_->evaluate(grasp, g_n);
00128
00129 probability += success_probability*probability_of_grasp_execution;
00130 }
00131
00132 probability /= normal_evaluator_->get_normalization_term();
00133
00134 return probability;
00135 }
00136
00137
00138 }