grasp_evaluator.cpp
Go to the documentation of this file.
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 #include <object_manipulation_msgs/GraspPlanning.h>
00036 
00037 #include "bayesian_grasp_planner/grasp_evaluator.h"
00038 #include "bayesian_grasp_planner/grasp_generator.h"
00039 #include "bayesian_grasp_planner/bayesian_grasp_planner_tools.h"
00040 
00041 namespace bayesian_grasp_planner {
00042 
00043 double RawGraspEvaluatorWithRegression::evaluate(const GraspWM &gi, 
00044                                                  const object_manipulation_msgs::GraspableObject &) const
00045 {
00046   double value = 0;
00047   //ROS_INFO("starting regression evaluator");
00048 
00049   //for drop-off function
00050   double dropoff_pos_bandwidth = 0.01; //1 cm std
00051   double dropoff_rot_bandwidth = 0.435; //25 degrees std
00052   
00053   //for smoothing regression
00054   double smoothing_pos_bandwidth = dropoff_pos_bandwidth / 2.0;
00055   double smoothing_rot_bandwidth = dropoff_rot_bandwidth / 2.0;
00056 
00057   //get grasps for the generator
00058   const std::vector<GraspWM> &grasps = generator_->getGrasps();
00059   //ROS_INFO("inside regression evaluator, got %d grasps for regression", (int)grasps.size());
00060 
00061   //compute regression contributions from all grasps and find the highest drop-off function value
00062   double cartesian_dist, rotation_dist;
00063   double energy = 0.0;
00064   double normalization_term = 0.0;
00065   double max_dropoff_value = 0.0;
00066   double regression_weight = 0.0;
00067   double dropoff_value = 0.0;
00068   for (size_t grasp_ind = 0; grasp_ind < grasps.size(); grasp_ind++)
00069   {
00070         grasps[grasp_ind].getDistance(gi, cartesian_dist, rotation_dist);
00071 
00072         if (cartesian_dist < dropoff_pos_bandwidth * 4 && rotation_dist < dropoff_rot_bandwidth * 4)
00073         {
00074           regression_weight = exp(-pow(cartesian_dist,2)/(2*pow(smoothing_pos_bandwidth,2))) * 
00075                                    exp(-pow(rotation_dist,2)/(2*pow(smoothing_rot_bandwidth,2)));
00076           dropoff_value =  exp(-pow(cartesian_dist,2)/(2*pow(dropoff_pos_bandwidth,2))) * 
00077                              exp(-pow(rotation_dist,2)/(2*pow(dropoff_rot_bandwidth,2)));
00078           if (dropoff_value > max_dropoff_value) max_dropoff_value = dropoff_value; 
00079           energy += regression_weight * grasps[grasp_ind].energy_function_score;
00080           normalization_term += regression_weight;
00081           //ROS_INFO("regression_weight: %0.3f", regression_weight);
00082           //ROS_INFO("dropoff_value: %0.3f", dropoff_value);
00083         }
00084   }
00085 
00086   //no grasps were close enough
00087   if (normalization_term == 0) 
00088   {
00089     //ROS_INFO("no grasps close enough (%zd tested)!", grasps.size());
00090     return 0.0;
00091   }     
00092   //take the minimum of the highest dropoff-value and the regression value
00093   value = std::min(energy/normalization_term, max_dropoff_value);  
00094   //ROS_INFO("grasp LWLR energy: %0.3f, max_dropoff_value: %0.3f, final value: %0.3f", 
00095   //         energy/normalization_term, max_dropoff_value, value);
00096   return value;
00097 }
00098 
00099 RawGraspEvaluatorServiceCaller::RawGraspEvaluatorServiceCaller(ros::NodeHandle &nh, std::string service_name, 
00100                                                                bool object_dependent)
00101 {
00102   object_dependent_ = object_dependent;
00103   service_ = register_service<object_manipulation_msgs::GraspPlanning>(nh, service_name);
00104 }
00105 
00106 double RawGraspEvaluatorServiceCaller::evaluate(const GraspWM &gi, 
00107                                                 const object_manipulation_msgs::GraspableObject &go) const
00108 {
00109   object_manipulation_msgs::GraspPlanning plan; 
00110   plan.request.target = go;
00111   plan.request.grasps_to_evaluate.push_back(gi.grasp_);
00112   if (!service_.call(plan))
00113   {
00114     ROS_ERROR("Grasp success probability computer, failed to call service at %s", service_name_.c_str());
00115     return 0.0;
00116   }
00117   ROS_ASSERT(plan.response.grasps.size() == 1);
00118   return plan.response.grasps[0].success_probability;  
00119 }
00120 
00121 void RawGraspEvaluatorServiceCaller::evaluate_list(std::vector<GraspWM> &grasps, 
00122                                                    const object_manipulation_msgs::GraspableObject &object,
00123                                                    std::vector<double> &values)
00124 {
00125   values.clear();
00126   values.resize(grasps.size(), 0.0);
00127   object_manipulation_msgs::GraspPlanning plan; 
00128   plan.request.target = object;
00129   BOOST_FOREACH(const GraspWM &grasp, grasps)
00130   {
00131     plan.request.grasps_to_evaluate.push_back(grasp.grasp_);
00132   }
00133   if (!service_.call(plan))
00134   {
00135     ROS_ERROR("Grasp success probability computer, failed to call service at %s", service_name_.c_str());
00136     return;
00137   }
00138   ROS_ASSERT(plan.response.grasps.size() == grasps.size());
00139   for (size_t i=0; i<grasps.size(); i++)
00140   {
00141     values[i] = plan.response.grasps[i].success_probability;
00142   }
00143 }
00144 
00145 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


bayesian_grasp_planner
Author(s): Kaijen Hsiao and Matei Ciocarlie
autogenerated on Fri Jan 25 2013 15:02:36