step_cost_key.cpp
Go to the documentation of this file.
00001 #include <vigir_footstep_planning_default_plugins/step_cost_estimators/step_cost_key.h>
00002 
00003 #include <boost/unordered_set.hpp>
00004 #include <boost/functional/hash.hpp>
00005 
00006 #include <tf/tf.h>
00007 
00008 
00009 
00010 namespace vigir_footstep_planning
00011 {
00012 StepCostKey::StepCostKey(const std::vector<double> &state, double cell_size, double angle_bin_size)
00013   : cell_size(cell_size)
00014   , angle_bin_size(angle_bin_size)
00015 {
00016   setState(state);
00017 }
00018 
00019 StepCostKey::StepCostKey(const State &left_foot, const State &right_foot, const State &swing_foot, double cell_size, double angle_bin_size)
00020   : cell_size(cell_size)
00021   , angle_bin_size(angle_bin_size)
00022 {
00023   std::vector<double> state;
00024   transformStates(left_foot, right_foot, swing_foot, state);
00025   setState(state);
00026 }
00027 
00028 StepCostKey::StepCostKey(const StepCostKey &other)
00029   : std::vector<int>(other)
00030   , cell_size(other.cell_size)
00031   , angle_bin_size(other.angle_bin_size)
00032 {
00033 }
00034 
00035 bool StepCostKey::operator== (const StepCostKey &other) const
00036 {
00037   unsigned long int size = std::min(this->size(), other.size());
00038 
00039   for (unsigned long int i = 0; i < size; i++)
00040     {
00041       if ((*this)[i] == other[i])
00042         continue;
00043 
00044       return false;
00045     }
00046 
00047   return true;
00048 }
00049 
00050 bool StepCostKey::operator< (const StepCostKey &other) const
00051 {
00052   unsigned long int size = std::min(this->size(), other.size());
00053 
00054   for (unsigned long int i = 0; i < size; i++)
00055     {
00056       if ((*this)[i] == other[i])
00057         continue;
00058 
00059       if ((*this)[i] < other[i])
00060         return true;
00061 
00062       if ((*this)[i] > other[i])
00063         return false;
00064     }
00065 
00066   return false;
00067 }
00068 
00069 void StepCostKey::setState(const std::vector<double> &state)
00070 {
00071   this->clear();
00072   this->reserve(state.size());
00073 
00074   unsigned int i = 0;
00075 
00076   this->push_back(cont_2_disc(state[i++], cell_size));
00077   this->push_back(cont_2_disc(state[i++], cell_size));
00078   //this->push_back(cont_2_disc(state[i++], cell_size));
00079   //this->push_back(cont_2_disc(state[i++], angle_bin_size));
00080   //this->push_back(cont_2_disc(state[i++], angle_bin_size));
00081   this->push_back(cont_2_disc(state[i++], angle_bin_size));
00082 
00083   this->push_back(cont_2_disc(state[i++], cell_size));
00084   this->push_back(cont_2_disc(state[i++], cell_size));
00085   //this->push_back(cont_2_disc(state[i++], cell_size));
00086   //this->push_back(cont_2_disc(state[i++], angle_bin_size));
00087   //this->push_back(cont_2_disc(state[i++], angle_bin_size));
00088   this->push_back(cont_2_disc(state[i++], angle_bin_size));
00089 
00090   //this->push_back(state[i++]);
00091 
00092   //ROS_INFO("%f %f %f | %f %f %f", state[0], state[1], state[2], state[3], state[4], state[5]);
00093   //ROS_INFO("%i %i %i | %i %i %i", (*this)[0], (*this)[1], (*this)[2], (*this)[3], (*this)[4], (*this)[5]);
00094   //ROS_INFO("--------");
00095 }
00096 
00097 void StepCostKey::getState(std::vector<double> &state) const
00098 {
00099   state.clear();
00100   state.reserve(this->size());
00101 
00102   unsigned int i = 0;
00103 
00104   state.push_back(disc_2_cont((*this)[i++], cell_size));
00105   state.push_back(disc_2_cont((*this)[i++], cell_size));
00106   //state.push_back(disc_2_cont((*this)[i++], cell_size));
00107   //state.push_back(disc_2_cont((*this)[i++], angle_bin_size));
00108   //state.push_back(disc_2_cont((*this)[i++], angle_bin_size));
00109   state.push_back(disc_2_cont((*this)[i++], angle_bin_size));
00110 
00111   state.push_back(disc_2_cont((*this)[i++], cell_size));
00112   state.push_back(disc_2_cont((*this)[i++], cell_size));
00113   //state.push_back(disc_2_cont((*this)[i++], cell_size));
00114   //state.push_back(disc_2_cont((*this)[i++], angle_bin_size));
00115   //state.push_back(disc_2_cont((*this)[i++], angle_bin_size));
00116   state.push_back(disc_2_cont((*this)[i++], angle_bin_size));
00117 
00118   //state.push_back((*this)[i++]);
00119 }
00120 
00121 void StepCostKey::transformStates(const State &left_foot, const State &right_foot, const State &swing_foot, std::vector<double> &state) const
00122 {
00123   /*
00124      * State definition:
00125      * A state is given by the left foot poses (before and after swinging).
00126      * Both poses must be given in the frame of the right standing foot.
00127      */
00128 
00129   tf::Pose left_foot_pose = left_foot.getPose();
00130   tf::Pose right_foot_pose = right_foot.getPose();
00131   tf::Pose swing_foot_pose = swing_foot.getPose();
00132 
00133   // transform left foot pose after executing the step relative to right foot
00134   tf::Pose left_foot_before;
00135   tf::Pose left_foot_after;
00136 
00137   // swing foot is left foot -> trivial
00138   if (swing_foot.getLeg() == LEFT)
00139   {
00140     // determine transformation, so right standing foot is origin
00141     tf::Transform world_to_right_foot = tf::Transform(right_foot_pose.getRotation(), right_foot_pose.getOrigin());
00142     world_to_right_foot = world_to_right_foot.inverse();
00143 
00144     // transform left standing foot pose into right foot frame
00145     left_foot_before = world_to_right_foot * left_foot_pose;
00146 
00147     // transform left swing pose into right foot frame
00148     left_foot_after = world_to_right_foot * swing_foot_pose;
00149   }
00150   // mirror all states to swinging left foot
00151   else // data_point.swing_foot.foot_index == RIGHT
00152   {
00153     // determine transformation, so left standing foot is origin
00154     tf::Transform world_to_left_foot = tf::Transform(left_foot_pose.getRotation(), left_foot_pose.getOrigin());
00155     world_to_left_foot = world_to_left_foot.inverse();
00156 
00157     // transform right standing foot pose into left foot frame
00158     tf::Pose right_foot_before;
00159     right_foot_before = world_to_left_foot * right_foot_pose;
00160 
00161     // transform right swing pose into left foot frame
00162     tf::Pose right_foot_after;
00163     right_foot_after = world_to_left_foot * swing_foot_pose;
00164 
00165     // now we are mirroring all data to right foot frame
00166     mirrorPoseOnXPlane(left_foot_before, right_foot_before);
00167     mirrorPoseOnXPlane(left_foot_after, right_foot_after);
00168   }
00169 
00170   state.clear();
00171   state.reserve(STATE_DIM);
00172   double r, p, y;
00173 
00174   // left foot before swing
00175   left_foot_before.getBasis().getRPY(r, p, y);
00176   state.push_back(left_foot_before.getOrigin().getX());
00177   state.push_back(left_foot_before.getOrigin().getY());
00178   //state.push_back(left_foot_before.getOrigin().getZ());
00179   //state.push_back(r);
00180   //state.push_back(p);
00181   state.push_back(y);
00182 
00183   // left foot after swing
00184   left_foot_after.getBasis().getRPY(r, p, y);
00185   state.push_back(left_foot_after.getOrigin().getX());
00186   state.push_back(left_foot_after.getOrigin().getY());
00187   //state.push_back(left_foot_after.getOrigin().getZ());
00188   //state.push_back(r);
00189   //state.push_back(p);
00190   state.push_back(y);
00191 
00192   // terrain type
00193   //state.push_back(data_point.terrain_type;
00194 
00195   //ROS_INFO("------- foot --------------------------");
00196   //ROS_INFO("%f %f %f / %f %f %f", state[0], state[1], state[2], state[3], state[4], state[5]);
00197   //ROS_INFO("%f %f %f / %f %f %f", state[6], state[7], state[8], state[9], state[10], state[11]);
00198 }
00199 
00200 void StepCostKey::mirrorPoseOnXPlane(tf::Pose &mirror, const tf::Pose &orig) const
00201 {
00202   mirror.setBasis(orig.getBasis());
00203   tf::Matrix3x3& basis = mirror.getBasis();
00204   basis[0][1] = -basis[0][1];
00205   basis[1][0] = -basis[1][0];
00206   basis[2][1] = -basis[2][1];
00207 
00208   mirror.setOrigin(orig.getOrigin());
00209   tf::Vector3& origin = mirror.getOrigin();
00210   origin[1] = -origin[1];
00211 
00212 
00213 //  double r, p, y;
00214 //  tf::Matrix3x3(orig.getRotation()).getRPY(r, p, y);
00215 //  mirror.setRotation(tf::createQuaternionFromRPY(-r, p, -y));
00216 
00217 //  tf::Vector3 v = orig.getOrigin();
00218 //  v.setY(-v.getY());
00219 //  mirror.setOrigin(v);
00220 
00221 
00222 //  tfScalar m[16];
00223 //  ROS_INFO("------------------");
00224 //  mirror.getOpenGLMatrix(m);
00225 //  ROS_INFO("%f %f %f %f", m[0], m[4], m[8], m[12]);
00226 //  ROS_INFO("%f %f %f %f", m[1], m[5], m[9], m[13]);
00227 //  ROS_INFO("%f %f %f %f", m[2], m[6], m[10], m[14]);
00228 //  ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);
00229 
00230 //  ROS_INFO("-");
00231 //  orig.getOpenGLMatrix(m);
00232 
00233 //  ROS_INFO("%f %f %f %f", m[0], m[4], m[8], m[12]);
00234 //  ROS_INFO("%f %f %f %f", m[1], m[5], m[9], m[13]);
00235 //  ROS_INFO("%f %f %f %f", m[2], m[6], m[10], m[14]);
00236 //  ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);
00237 
00238 //  ROS_INFO("-");
00239 
00240 //  ROS_INFO("%f %f %f %f", m[0], -m[4], m[8], m[12]);
00241 //  ROS_INFO("%f %f %f %f", -m[1], m[5], m[9], -m[13]);
00242 //  ROS_INFO("%f %f %f %f", m[2], -m[6], m[10], m[14]);
00243 //  ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);
00244 }
00245 }
00246 
00247 std::size_t hash_value(const vigir_footstep_planning::StepCostKey &key)
00248 {
00249   std::size_t seed = 0;
00250   for (unsigned int i = 0; i < STATE_DIM; i++)
00251   {
00252     boost::hash_combine(seed, key[i]);
00253   }
00254   return seed;
00255 }


vigir_footstep_planning_default_plugins
Author(s): Alexander Stumpf
autogenerated on Thu Jun 6 2019 18:38:06