$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 #include <boost/foreach.hpp> 00038 00039 //#define PROF_ENABLED 00040 //#include <profiling/profiling.h> 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 //PROF_DECLARE(PRGSPC_PROF); 00047 //PROF_DECLARE(SIMPLE_GSPC_PROF); 00048 00049 namespace probabilistic_grasp_planner { 00050 00051 00052 double SimpleGraspSuccessProbabilityComputer::getProbability(const GraspWithMetadata &grasp) 00053 { 00054 //PROF_TIMER_FUNC(SIMPLE_GSPC_PROF); 00055 // if grasp is not from object, return 0 00056 if (model_id_ != grasp.model_id_) return 0.0; 00057 // otherwise, return non-zero exponentially decaying probability 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 //PROF_TIMER_FUNC(PRGSPC_//PROF); 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 //Compute the probability of success of that perturbation 00125 double success_probability = probability_computer_->getProbability(g_n); 00126 //Compute the likelihood of getting to that perturbation given the originally commanded grasp 00127 double probability_of_grasp_execution = normal_evaluator_->evaluate(grasp, g_n); 00128 //Add the product of these two values to the resulting probability 00129 probability += success_probability*probability_of_grasp_execution; 00130 } 00131 //Normalize to turn likelihoods into probabilities 00132 probability /= normal_evaluator_->get_normalization_term(); 00133 //Return the sum 00134 return probability; 00135 } 00136 00137 00138 }