step_cost_key.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2017, Alexander Stumpf, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef VIGIR_FOOTSTEP_PLANNING_STEP_COST_KEY_H
00030 #define VIGIR_FOOTSTEP_PLANNING_STEP_COST_KEY_H
00031 
00032 #include <vigir_footstep_planning_lib/modeling/state.h>
00033 
00034 #define STATE_DIM 6
00035 
00036 
00037 
00038 namespace vigir_footstep_planning
00039 {
00040 class StepCostKey
00041   : public std::vector<int>
00042 {
00043 public:
00044   StepCostKey(const std::vector<double> &state, double cell_size, double angle_bin_size);
00045   StepCostKey(const State &left_foot, const State &right_foot, const State &swing_foot, double cell_size, double angle_bin_size);
00046   StepCostKey(const StepCostKey &other);
00047 
00048   bool operator== (const StepCostKey &other) const;
00049   bool operator< (const StepCostKey &other) const;
00050 
00051   void setState(const std::vector<double> &state);
00052   void getState(std::vector<double> &state) const;
00053 
00054 protected:
00055   void transformStates(const State &left_foot, const State &right_foot, const State &swing_foot, std::vector<double> &state) const;
00056   void mirrorPoseOnXPlane(tf::Pose &mirror, const tf::Pose &orig) const;
00057 
00059   inline int cont_2_disc(double value, double cell_size) const
00060   {
00061     return int(round(value / cell_size));
00062   }
00063 
00065   inline double disc_2_cont(int value, double cell_size) const
00066   {
00067     return double(value) * cell_size;
00068   }
00069 
00070   const double cell_size, angle_bin_size;
00071 };
00072 }
00073 
00074 std::size_t hash_value(const vigir_footstep_planning::StepCostKey &key);
00075 
00076 #endif


vigir_footstep_planning_default_plugins
Author(s): Alexander Stumpf
autogenerated on Fri Apr 7 2017 02:59:40