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


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Sat Jun 8 2019 20:21:05