footstep.cpp
Go to the documentation of this file.
00001 // SVN $HeadURL: https://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_navigation/footstep_planner/src/Dstar.cpp $
00002 // SVN $Id: Dstar.cpp 1168 2011-03-30 03:18:02Z 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  * D* Lite (Koenig et al. 2002) partly based on the implementation
00011  * by J. Neufeld (http://code.google.com/p/dstarlite/)
00012  *
00013  *
00014  * This program is free software: you can redistribute it and/or modify
00015  * it under the terms of the GNU General Public License as published by
00016  * the Free Software Foundation, version 3.
00017  *
00018  * This program is distributed in the hope that it will be useful,
00019  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00020  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00021  * GNU General Public License for more details.
00022  *
00023  * You should have received a copy of the GNU General Public License
00024  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00025  */
00026 
00027 #include <vigir_footstep_planning_lib/modeling/footstep.h>
00028 
00029 
00030 namespace vigir_footstep_planning
00031 {
00032 Footstep::Footstep(double x, double y, double theta, double step_cost, double cell_size, int num_angle_bins, int max_hash_size)
00033   : ivCellSize(cell_size)
00034   , ivNumAngleBins(num_angle_bins)
00035   , ivAngleBinSize(2.0*M_PI / ivNumAngleBins)
00036   , ivTheta(angle_state_2_cell(theta, ivAngleBinSize))
00037   , ivStepCost(step_cost)
00038   , ivMaxHashSize(max_hash_size)
00039   , ivDiscSuccessorLeft(num_angle_bins)
00040   , ivDiscSuccessorRight(num_angle_bins)
00041   , ivDiscPredecessorLeft(num_angle_bins)
00042   , ivDiscPredecessorRight(num_angle_bins)
00043 {
00044   init(x, y);
00045 }
00046 
00047 Footstep::~Footstep()
00048 {}
00049 
00050 void Footstep::init(double x, double y)
00051 {
00052   int backward_angle;
00053   int footstep_x;
00054   int footstep_y;
00055 
00056   for (int a = 0; a < ivNumAngleBins; ++a)
00057   {
00058     backward_angle = calculateForwardStep(RIGHT, a, x, y, &footstep_x, &footstep_y);
00059     ivDiscSuccessorRight[a] = footstep_xy(footstep_x, footstep_y);
00060     ivDiscPredecessorLeft[backward_angle] = footstep_xy(-footstep_x, -footstep_y);
00061 
00062     backward_angle = calculateForwardStep(LEFT, a, x, y, &footstep_x, &footstep_y);
00063     ivDiscSuccessorLeft[a] = footstep_xy(footstep_x, footstep_y);
00064     ivDiscPredecessorRight[backward_angle] = footstep_xy(-footstep_x, -footstep_y);
00065   }
00066 }
00067 
00068 PlanningState Footstep::performMeOnThisState(const PlanningState& current) const
00069 {
00070   assert(current.getPredState() != nullptr);
00071 
00072   const State& left = (current.getLeg() == LEFT) ? current.getState() : current.getPredState()->getState();
00073   const State& right = (current.getLeg() == RIGHT) ? current.getState() : current.getPredState()->getState();
00074 
00075   State swing = current.getState();
00076   int x = current.getX();
00077   int y = current.getY();
00078   int yaw = current.getYaw();
00079 
00080   // theta has to be in [0..ivNumAngleBins)
00081   if (yaw < 0)
00082     yaw += ivNumAngleBins;
00083   else if (yaw >= ivNumAngleBins)
00084     yaw -= ivNumAngleBins;
00085 
00086   if (current.getLeg() == RIGHT)
00087   {
00088     footstep_xy xy = ivDiscSuccessorRight[yaw];
00089     x += xy.first;
00090     y += xy.second;
00091     yaw += ivTheta;
00092     swing.setLeg(LEFT);
00093   }
00094   else // leg == LEFT
00095   {
00096     footstep_xy xy = ivDiscSuccessorLeft[yaw];
00097     x += xy.first;
00098     y += xy.second;
00099     yaw -= ivTheta;
00100     swing.setLeg(RIGHT);
00101   }
00102 
00103   swing.setX(cell_2_state(x, ivCellSize));
00104   swing.setY(cell_2_state(y, ivCellSize));
00105   swing.setZ(current.getState().getZ());
00106 
00107   double yaw_d = angles::normalize_angle(angle_cell_2_state(yaw, ivAngleBinSize));
00108   swing.setYaw(yaw_d);
00109 
00110   return PlanningState(swing, ivCellSize, ivAngleBinSize, ivMaxHashSize, &current, nullptr);
00111 }
00112 
00113 PlanningState Footstep::reverseMeOnThisState(const PlanningState& current) const
00114 {
00115   assert(current.getSuccState() != nullptr);
00116 
00117   const State& left = (current.getLeg() == LEFT) ? current.getState() : current.getSuccState()->getState();
00118   const State& right = (current.getLeg() == RIGHT) ? current.getState() : current.getSuccState()->getState();
00119 
00120   State swing = current.getState();
00121   int x = current.getX();
00122   int y = current.getY();
00123   int yaw = current.getYaw();
00124 
00125   // theta has to be in [0..ivNumAngleBins)
00126   if (yaw < 0)
00127     yaw += ivNumAngleBins;
00128   else if (yaw >= ivNumAngleBins)
00129     yaw -= ivNumAngleBins;
00130 
00131   if (current.getLeg() == RIGHT)
00132   {
00133     footstep_xy xy = ivDiscPredecessorRight[yaw];
00134     x += xy.first;
00135     y += xy.second;
00136     yaw += ivTheta;
00137     swing.setLeg(LEFT);
00138   }
00139   else // leg == LEFT
00140   {
00141     footstep_xy xy = ivDiscPredecessorLeft[yaw];
00142     x += xy.first;
00143     y += xy.second;
00144     yaw -= ivTheta;
00145     swing.setLeg(RIGHT);
00146   }
00147 
00148   swing.setX(cell_2_state(x, ivCellSize));
00149   swing.setY(cell_2_state(y, ivCellSize));
00150   swing.setZ(current.getState().getZ());
00151 
00152   double yaw_d = angles::normalize_angle(angle_cell_2_state(yaw, ivAngleBinSize));
00153   swing.setYaw(yaw_d);
00154 
00155   return PlanningState(swing, ivCellSize, ivAngleBinSize, ivMaxHashSize, nullptr, &current);
00156 }
00157 
00158 int Footstep::calculateForwardStep(Leg leg, int global_theta, double x, double y, int* footstep_x, int* footstep_y) const
00159 {
00160   double cont_footstep_x, cont_footstep_y;
00161   double cont_global_theta = angle_cell_2_state(global_theta, ivAngleBinSize);
00162   double theta_cos = cos(cont_global_theta);
00163   double theta_sin = sin(cont_global_theta);
00164   if (leg == RIGHT)
00165   {
00166     cont_footstep_x = theta_cos * x - theta_sin * y;
00167     cont_footstep_y = theta_sin * x + theta_cos * y;
00168 
00169     global_theta += ivTheta;
00170   }
00171   else // leg == LEFT
00172   {
00173     cont_footstep_x = theta_cos * x + theta_sin * y;
00174     cont_footstep_y = theta_sin * x - theta_cos * y;
00175 
00176     global_theta -= ivTheta;
00177   }
00178   *footstep_x = disc_val(cont_footstep_x, ivCellSize);
00179   *footstep_y = disc_val(cont_footstep_y, ivCellSize);
00180 
00181   // theta has to be in [0..ivNumAngleBins)
00182   if (global_theta < 0)
00183     global_theta += ivNumAngleBins;
00184   else if (global_theta >= ivNumAngleBins)
00185     global_theta -= ivNumAngleBins;
00186   return global_theta;
00187 }
00188 } // end of namespace


vigir_footstep_planning_lib
Author(s): Alexander Stumpf
autogenerated on Sat Jul 15 2017 02:47:56