footstep_planner: footstep_planner: Heuristic.cpp Source File

00001 // SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/footstep_planner/src/Heuristic.cpp $
00002 // SVN $Id: Heuristic.cpp 3298 2012-09-28 11:37:38Z hornunga@informatik.uni-freiburg.de $
00003 
00004 /*
00005  * A footstep planner for humanoid robots
00006  *
00007  * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
00008  * http://www.ros.org/wiki/footstep_planner
00009  *
00010  *
00011  * This program is free software: you can redistribute it and/or modify
00012  * it under the terms of the GNU General Public License as published by
00013  * the Free Software Foundation, version 3.
00014  *
00015  * This program is distributed in the hope that it will be useful,
00016  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  * GNU General Public License for more details.
00019  *
00020  * You should have received a copy of the GNU General Public License
00021  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00022  */
00023 
00024 #include <footstep_planner/Heuristic.h>
00025 
00026 namespace footstep_planner
00027 {
00028 Heuristic::Heuristic(double cell_size, int num_angle_bins,
00029                      HeuristicType type)
00030 : ivCellSize(cell_size),
00031   ivNumAngleBins(num_angle_bins),
00032   ivHeuristicType(type)
00033 {}
00034 
00035 
00036 Heuristic::~Heuristic()
00037 {}
00038 
00039 
00040 EuclideanHeuristic::EuclideanHeuristic(double cell_size, int num_angle_bins)
00041 : Heuristic(cell_size, num_angle_bins, EUCLIDEAN)
00042 {}
00043 
00044 
00045 EuclideanHeuristic::~EuclideanHeuristic()
00046 {}
00047 
00048 
00049 double
00050 EuclideanHeuristic::getHValue(const PlanningState& from,
00051                               const PlanningState& to)
00052 const
00053 {
00054   if (from == to)
00055     return 0.0;
00056 
00057   // distance in cell size
00058   double dist = euclidean_distance(from.getX(), from.getY(),
00059                                    to.getX(), to.getY());
00060   // return distance in meter
00061   return cont_val(dist, ivCellSize);
00062 }
00063 
00064 
00065 EuclStepCostHeuristic::EuclStepCostHeuristic(double cell_size,
00066                                              int    num_angle_bins,
00067                                              double step_cost,
00068                                              double diff_angle_cost,
00069                                              double max_step_width)
00070 : Heuristic(cell_size, num_angle_bins, EUCLIDEAN_STEPCOST),
00071   ivStepCost(step_cost),
00072   ivDiffAngleCost(diff_angle_cost),
00073   ivMaxStepWidth(max_step_width)
00074 {}
00075 
00076 
00077 EuclStepCostHeuristic::~EuclStepCostHeuristic()
00078 {}
00079 
00080 
00081 double
00082 EuclStepCostHeuristic::getHValue(const PlanningState& from,
00083                                  const PlanningState& to)
00084 const
00085 {
00086   if (from == to)
00087     return 0.0;
00088 
00089   // distance in meter
00090   double dist = cont_val(euclidean_distance(
00091       from.getX(), from.getY(), to.getX(), to.getY()), ivCellSize);
00092   double expected_steps = dist / ivMaxStepWidth;
00093   double diff_angle = 0.0;
00094   if (ivDiffAngleCost > 0.0)
00095   {
00096     // get the number of bins between from.theta and to.theta
00097     int diff_angle_disc = (
00098         ((to.getTheta() - from.getTheta()) % ivNumAngleBins) +
00099         ivNumAngleBins) % ivNumAngleBins;
00100     // get the rotation independent from the rotation direction
00101     diff_angle = std::abs(angles::normalize_angle(
00102         angle_cell_2_state(diff_angle_disc, ivNumAngleBins)));
00103   }
00104 
00105   return (dist + expected_steps * ivStepCost +
00106       diff_angle * ivDiffAngleCost);
00107 }
00108 }