Footstep.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/Footstep.h>
00022 
00023 
00024 namespace footstep_planner
00025 {
00026 Footstep::Footstep(double x, double y, double theta, double cell_size,
00027                    int num_angle_bins, int max_hash_size)
00028 : ivTheta(angle_state_2_cell(theta, num_angle_bins)),
00029   ivCellSize(cell_size),
00030   ivNumAngleBins(num_angle_bins),
00031   ivMaxHashSize(max_hash_size),
00032   ivDiscSuccessorLeft(num_angle_bins),
00033   ivDiscSuccessorRight(num_angle_bins),
00034   ivDiscPredecessorLeft(num_angle_bins),
00035   ivDiscPredecessorRight(num_angle_bins)
00036 {
00037   init(x, y);
00038 }
00039 
00040 
00041 Footstep::~Footstep()
00042 {}
00043 
00044 
00045 void
00046 Footstep::init(double x, double y)
00047 {
00048   int backward_angle;
00049   int footstep_x;
00050   int footstep_y;
00051 
00052   for (int a = 0; a < ivNumAngleBins; ++a)
00053   {
00054     backward_angle = calculateForwardStep(RIGHT, a, x, y,
00055                                           &footstep_x, &footstep_y);
00056     ivDiscSuccessorRight[a] = footstep_xy(footstep_x, footstep_y);
00057     ivDiscPredecessorLeft[backward_angle] = footstep_xy(-footstep_x,
00058                                                         -footstep_y);
00059     backward_angle = calculateForwardStep(LEFT, a, x, y,
00060                                           &footstep_x, &footstep_y);
00061     ivDiscSuccessorLeft[a] = footstep_xy(footstep_x, footstep_y);
00062     ivDiscPredecessorRight[backward_angle] = footstep_xy(-footstep_x,
00063                                                          -footstep_y);
00064   }
00065 }
00066 
00067 
00068 PlanningState
00069 Footstep::performMeOnThisState(const PlanningState& current)
00070 const
00071 {
00072   Leg leg;
00073 
00074   int x = current.getX();
00075   int y = current.getY();
00076   int theta = current.getTheta();
00077 
00078   if (current.getLeg() == RIGHT)
00079   {
00080     footstep_xy xy = ivDiscSuccessorRight[theta];
00081     x += xy.first;
00082     y += xy.second;
00083     theta += ivTheta;
00084     leg = LEFT;
00085   }
00086   else // leg == LEFT
00087   {
00088     footstep_xy xy = ivDiscSuccessorLeft[theta];
00089     x += xy.first;
00090     y += xy.second;
00091     theta -= ivTheta;
00092     leg = RIGHT;
00093   }
00094 
00095   // theta has to be in [0..ivNumAngleBins)
00096   if (theta < 0)
00097     theta += ivNumAngleBins;
00098   else if (theta >= ivNumAngleBins)
00099     theta -= ivNumAngleBins;
00100 
00101   return PlanningState(x, y, theta, leg, ivMaxHashSize);
00102 }
00103 
00104 
00105 PlanningState
00106 Footstep::reverseMeOnThisState(const PlanningState& current)
00107 const
00108 {
00109   Leg leg;
00110 
00111   int x = current.getX();
00112   int y = current.getY();
00113   int theta = current.getTheta();
00114 
00115   if (current.getLeg() == LEFT)
00116   {
00117     footstep_xy xy = ivDiscPredecessorLeft[theta];
00118     x += xy.first;
00119     y += xy.second;
00120     theta -= ivTheta;
00121     leg = RIGHT;
00122   }
00123   else // leg == RIGHT
00124   {
00125     footstep_xy xy = ivDiscPredecessorRight[theta];
00126     x += xy.first;
00127     y += xy.second;
00128     theta += ivTheta;
00129     leg = LEFT;
00130   }
00131   // theta has to be in [0..ivNumAngleBins)
00132   if (theta < 0)
00133     theta += ivNumAngleBins;
00134   else if (theta >= ivNumAngleBins)
00135     theta -= ivNumAngleBins;
00136 
00137   return PlanningState(x, y, theta, leg, ivMaxHashSize);
00138 }
00139 
00140 
00141 int
00142 Footstep::calculateForwardStep(Leg leg, int global_theta,
00143                                double x, double y,
00144                                int* footstep_x, int* footstep_y)
00145 const
00146 {
00147   double cont_footstep_x, cont_footstep_y;
00148   double cont_global_theta = angle_cell_2_state(global_theta,
00149                                                 ivNumAngleBins);
00150   double theta_cos = cos(cont_global_theta);
00151   double theta_sin = sin(cont_global_theta);
00152   if (leg == RIGHT)
00153   {
00154     cont_footstep_x = theta_cos * x - theta_sin * y;
00155     cont_footstep_y = theta_sin * x + theta_cos * y;
00156 
00157     global_theta += ivTheta;
00158   }
00159   else // leg == LEFT
00160       {
00161     cont_footstep_x = theta_cos * x + theta_sin * y;
00162     cont_footstep_y = theta_sin * x - theta_cos * y;
00163 
00164     global_theta -= ivTheta;
00165       }
00166   *footstep_x = disc_val(cont_footstep_x, ivCellSize);
00167   *footstep_y = disc_val(cont_footstep_y, ivCellSize);
00168 
00169   // theta has to be in [0..ivNumAngleBins)
00170   if (global_theta < 0)
00171     global_theta += ivNumAngleBins;
00172   else if (global_theta >= ivNumAngleBins)
00173     global_theta -= ivNumAngleBins;
00174   return global_theta;
00175 }
00176 } // end of namespace


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Mon Oct 6 2014 00:41:41