Heuristic.cpp
Go to the documentation of this file.
1 /*
2  * A footstep planner for humanoid robots
3  *
4  * Copyright 2010-2011 Johannes Garimort, Armin Hornung, University of Freiburg
5  * http://www.ros.org/wiki/footstep_planner
6  *
7  *
8  * This program is free software: you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation, version 3.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program. If not, see <http://www.gnu.org/licenses/>.
19  */
20 
22 
23 namespace footstep_planner
24 {
25 Heuristic::Heuristic(double cell_size, int num_angle_bins,
26  HeuristicType type)
27 : ivCellSize(cell_size),
28  ivNumAngleBins(num_angle_bins),
29  ivHeuristicType(type)
30 {}
31 
32 
34 {}
35 
36 
37 EuclideanHeuristic::EuclideanHeuristic(double cell_size, int num_angle_bins)
38 : Heuristic(cell_size, num_angle_bins, EUCLIDEAN)
39 {}
40 
41 
43 {}
44 
45 
46 double
48  const PlanningState& to)
49 const
50 {
51  if (from == to)
52  return 0.0;
53 
54  // distance in cell size
55  double dist = euclidean_distance(from.getX(), from.getY(),
56  to.getX(), to.getY());
57  // return distance in meter
58  return cont_val(dist, ivCellSize);
59 }
60 
61 
63  int num_angle_bins,
64  double step_cost,
65  double diff_angle_cost,
66  double max_step_width)
67 : Heuristic(cell_size, num_angle_bins, EUCLIDEAN_STEPCOST),
68  ivStepCost(step_cost),
69  ivDiffAngleCost(diff_angle_cost),
70  ivMaxStepWidth(max_step_width)
71 {}
72 
73 
75 {}
76 
77 
78 double
80  const PlanningState& to)
81 const
82 {
83  if (from == to)
84  return 0.0;
85 
86  // distance in meter
87  double dist = cont_val(euclidean_distance(
88  from.getX(), from.getY(), to.getX(), to.getY()), ivCellSize);
89  double expected_steps = dist / ivMaxStepWidth;
90  double diff_angle = 0.0;
91  if (ivDiffAngleCost > 0.0)
92  {
93  // get the number of bins between from.theta and to.theta
94  int diff_angle_disc = (
95  ((to.getTheta() - from.getTheta()) % ivNumAngleBins) +
97  // get the rotation independent from the rotation direction
98  diff_angle = std::abs(angles::normalize_angle(
99  angle_cell_2_state(diff_angle_disc, ivNumAngleBins)));
100  }
101 
102  return (dist + expected_steps * ivStepCost +
103  diff_angle * ivDiffAngleCost);
104 }
105 }
EuclideanHeuristic(double cell_size, int num_angle_bins)
Definition: Heuristic.cpp:37
virtual double getHValue(const PlanningState &from, const PlanningState &to) const
Definition: Heuristic.cpp:47
double euclidean_distance(int x1, int y1, int x2, int y2)
Definition: helper.h:56
An abstract super class providing methods necessary to be used as heuristic function within the Foots...
Definition: Heuristic.h:34
EuclStepCostHeuristic(double cell_size, int num_angle_bins, double step_cost, double diff_angle_cost, double max_step_width)
Definition: Heuristic.cpp:62
double angle_cell_2_state(int angle, int angle_bin_num)
Calculate the respective (continuous) angle for a bin.
Definition: helper.h:101
def normalize_angle(angle)
Heuristic(double cell_size, int num_angle_bins, HeuristicType type)
Definition: Heuristic.cpp:25
const double ivMaxStepWidth
longest step width
Definition: Heuristic.h:99
A class representing the robot&#39;s pose (i.e. position and orientation) in the underlying SBPL...
Definition: PlanningState.h:45
virtual double getHValue(const PlanningState &from, const PlanningState &to) const
Definition: Heuristic.cpp:79
double cont_val(int length, double cell_size)
Calculates the continuous value for a length discretized in cell size.
Definition: helper.h:141


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Mon Jun 10 2019 13:38:24