probabilistic_grasp_planner: distribution

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 
00036 
00037 // Bring in my package's API, which is what I'm testing
00038 #include "probabilistic_grasp_planner/distribution_evaluator.h"
00039 #include "probabilistic_grasp_planner/probabilistic_planner_tools.h"
00040 // Bring in gtest
00041 #include <gtest/gtest.h>
00042 
00043 // Declare a test
00044 TEST(DistributionEvaluator, testUniformDistribution)
00045 {
00046   probabilistic_grasp_planner::UniformDistributionEvaluator evaluator;
00047 
00048   probabilistic_grasp_planner::GraspWithMetadata grasp;
00049   EXPECT_DOUBLE_EQ(evaluator.get_normalization_term(),0);
00050   double res = evaluator.evaluate(grasp,grasp);
00051   EXPECT_DOUBLE_EQ(res, 1.0);
00052 
00053 }
00054 
00055 
00056 // Declare another test
00057 TEST(DistributionEvaluator, testNormalDistribution)
00058 {
00059   double position_sigma = 0.01;
00060   double orientation_concentration = 10.0;
00061 
00062   probabilistic_grasp_planner::NormalDistributionEvaluator evaluator2(position_sigma, orientation_concentration);
00063   EXPECT_DOUBLE_EQ(evaluator2.get_normalization_term(),0);
00064 
00065   probabilistic_grasp_planner::GraspWithMetadata gstar;
00066   gstar.tool_point_pose_.setIdentity();
00067 
00068   probabilistic_grasp_planner::GraspWithMetadata grasp1;
00069   grasp1.tool_point_pose_.setIdentity();
00070   grasp1.tool_point_pose_.setOrigin(tf::Vector3(0.01,0,0));
00071 
00072   double res = evaluator2.evaluate(gstar,grasp1);
00073   EXPECT_DOUBLE_EQ(res, 0.60653065971263342);
00074 
00075   probabilistic_grasp_planner::GraspWithMetadata grasp2;
00076   grasp2.tool_point_pose_.setIdentity();
00077   grasp2.tool_point_pose_.setOrigin(tf::Vector3(0.01,0.01,0));
00078   res = evaluator2.evaluate(gstar,grasp2);
00079   EXPECT_DOUBLE_EQ(res, 0.36787944117144233);
00080 
00081   probabilistic_grasp_planner::GraspWithMetadata grasp3;
00082   grasp3.tool_point_pose_.setIdentity();
00083   tf::Matrix3x3 basis;
00084   basis.setEulerYPR(0.0,0.0,0.0);
00085   grasp3.tool_point_pose_.setBasis(basis);
00086   res = evaluator2.evaluate(gstar,grasp3);
00087   EXPECT_DOUBLE_EQ(res, 1.0);
00088 
00089   basis.setEulerYPR(0.0,0.0,M_PI_4);
00090   grasp3.tool_point_pose_.setBasis(basis);
00091   res = evaluator2.evaluate(gstar,grasp3);
00092   EXPECT_DOUBLE_EQ(res, 0.23120139832879863);
00093 
00094 }
00095 
00096 //TEST(TestSuite, )
00097 
00098 // Run all the tests that were declared with TEST()
00099 int main(int argc, char **argv)
00100 {
00101   testing::InitGoogleTest(&argc, argv);
00102   return RUN_ALL_TESTS();
00103 }