environment_chain3d_types.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Maxim Likhachev
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Maxim Likhachev nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 
00038 #ifndef _ENVIRONMENT_CHAIN3D_TYPES_H_
00039 #define _ENVIRONMENT_CHAIN3D_TYPES_H_
00040 
00041 #include <vector>
00042 #include <planning_models/robot_model.h>
00043 #include <planning_models/angle_utils.h>
00044 
00045 namespace sbpl_interface {
00046 
00047 static unsigned int HASH_TABLE_SIZE = 32*1024;
00048 
00049 static inline unsigned int intHash(unsigned int key)
00050 {
00051   key += (key << 12);
00052   key ^= (key >> 22);
00053   key += (key << 4);
00054   key ^= (key >> 9);
00055   key += (key << 10);
00056   key ^= (key >> 2);
00057   key += (key << 7);
00058   key ^= (key >> 12);
00059   return key;
00060 }
00061 struct EnvChain3DHashEntry
00062 {
00063   unsigned char dist; // distance to closest obstacle
00064   int stateID; // hash entry ID number
00065   int action; // which action in the list was required to get here
00066   int xyz[3]; // tip link coordinates in space
00067   std::vector<int> coord; // position in the angle discretization
00068   std::vector<double> angles; // position of joints in continuous space
00069 };
00070 
00072 struct EnvChain3DGoalPose
00073 {
00074   bool is_6dof_goal;
00075   int type;
00076   int xyz_disc_tolerance;
00077   int rpy_disc_tolerance;
00078   int xyz_disc[3];
00079   int rpy_disc[3];
00080   double xyz[3];
00081   double rpy[3];
00082   double q[4];
00083   double fangle;
00084   double xyz_tolerance[3];
00085   double rpy_tolerance[3];
00086 };
00087 
00089 struct EnvChain3DPlanningData
00090 {
00091 
00092   EnvChain3DPlanningData(std::vector<int*>& state_ID_to_index_mapping) :
00093     state_ID_to_index_mapping_(state_ID_to_index_mapping),
00094     goal_hash_entry_(NULL),
00095     start_hash_entry_(NULL),
00096     hash_table_size_(HASH_TABLE_SIZE)
00097   {
00098     coord_to_state_ID_table_.resize(hash_table_size_);
00099   }
00100 
00101   ~EnvChain3DPlanningData() {
00102     for(unsigned int i = 0; i < state_ID_to_coord_table_.size(); i++) {
00103       delete state_ID_to_coord_table_[i];
00104     }
00105   }
00106 
00107   unsigned int getHashBin(const std::vector<int>& coord) {
00108     unsigned int val = 0;
00109 
00110     for(size_t i = 0; i < coord.size(); i++)
00111       val += intHash(coord[i]) << i;
00112 
00113     return intHash(val) & (hash_table_size_-1);
00114   }
00115 
00116   EnvChain3DHashEntry* addHashEntry(const std::vector<int>& coord,
00117                                     const std::vector<double>& angles,
00118                                     const int(&xyz)[3],
00119                                     int action)
00120   {
00121     EnvChain3DHashEntry* new_hash_entry = new EnvChain3DHashEntry();
00122     new_hash_entry->stateID = state_ID_to_coord_table_.size();
00123     new_hash_entry->coord = coord;
00124     new_hash_entry->angles = angles;
00125     memcpy(new_hash_entry->xyz, xyz, sizeof(int)*3);
00126     new_hash_entry->action = action;
00127     state_ID_to_coord_table_.push_back(new_hash_entry);
00128     unsigned int bin = getHashBin(coord);
00129     coord_to_state_ID_table_[bin].push_back(new_hash_entry);
00130 
00131     //have to do for DiscreteSpaceInformation
00132     //insert into and initialize the mappings
00133     int* entry = new int [NUMOFINDICES_STATEID2IND];
00134     memset(entry, -1, NUMOFINDICES_STATEID2IND*sizeof(int));
00135     state_ID_to_index_mapping_.push_back(entry);
00136     if(new_hash_entry->stateID != (int)state_ID_to_index_mapping_.size()-1) {
00137       ROS_ERROR_STREAM("Size mismatch between state mappings " << new_hash_entry->stateID << " " << state_ID_to_index_mapping_.size());
00138     }
00139     return new_hash_entry;
00140   }
00141 
00142   EnvChain3DHashEntry* getHashEntry(const std::vector<int> &coord,
00143                                     int action)
00144   {
00145     unsigned int bin = getHashBin(coord);
00146     for(unsigned int i = 0; i < coord_to_state_ID_table_[bin].size(); i++) {
00147       if(coord_to_state_ID_table_[bin][i]->coord == coord) {
00148         return coord_to_state_ID_table_[bin][i];
00149       }
00150     }
00151     return NULL;
00152   }
00153 
00154   bool convertFromStateIDsToAngles(const std::vector<int>& state_ids,
00155                                    std::vector<std::vector<double> >& angle_vector) const {
00156     angle_vector.resize(state_ids.size());
00157     for(unsigned int i = 0; i < state_ids.size(); i++) {
00158       if(state_ids[i] > (int) state_ID_to_coord_table_.size()-1) {
00159         return false;
00160       }
00161       angle_vector[i] = state_ID_to_coord_table_[state_ids[i]]->angles;
00162     }
00163     return true;
00164   }
00165 
00166   //internal data from DiscreteSpaceInformation
00167   std::vector<int*>& state_ID_to_index_mapping_;
00168 
00169   EnvChain3DHashEntry* goal_hash_entry_;
00170   EnvChain3DHashEntry* start_hash_entry_;
00171 
00172   unsigned int hash_table_size_;
00173   //maps from coords to stateID
00174   std::vector<std::vector<EnvChain3DHashEntry*> > coord_to_state_ID_table_;
00175 
00176   //vector that maps from stateID to coords
00177   std::vector<EnvChain3DHashEntry*> state_ID_to_coord_table_;
00178 
00179 };
00180 
00181 class JointMotionWrapper {
00182 public:
00183 
00184   JointMotionWrapper(const planning_models::RobotModel::JointModel* joint_model) :
00185     joint_model_(joint_model)
00186   {
00187     std::vector<moveit_msgs::JointLimits> limits = joint_model->getLimits();
00188     joint_limit_ = limits.front();
00189   }
00190 
00191   bool getSuccessorValue(double start,
00192                          double delta,
00193                          double& end) {
00194     end = start+delta;
00195     if(joint_limit_.has_position_limits) {
00196       if(end < joint_limit_.min_position) {
00197         if(fabs(start-joint_limit_.min_position) > std::numeric_limits<double>::epsilon()) {
00198           //start not at min limit, so peg the end position
00199           end = joint_limit_.min_position;
00200         } else {
00201           //start already at min limit
00202           return false;
00203         }
00204       } else if(end > joint_limit_.max_position) {
00205         if(fabs(start-joint_limit_.max_position) > std::numeric_limits<double>::epsilon()) {
00206           //start not at max limit, so peg the end position
00207           end = joint_limit_.max_position;
00208         } else {
00209           //start already at max limit
00210           return false;
00211         }
00212       }
00213     } else {
00214       end = normalizeAngle(end);
00215     }
00216     return true;
00217   }
00218 
00219   bool canGetCloser(double start,
00220                     double goal,
00221                     double delta) {
00222     double start_dist = getDoubleDistance(start,goal);
00223     double plus_value = 0.0;
00224     double minus_value = 0.0;
00225     if(getSuccessorValue(start, delta, plus_value)) {
00226       double succ_dist = getDoubleDistance(plus_value, goal);
00227       if(succ_dist < start_dist) {
00228         return true;
00229       }
00230     }
00231     if(getSuccessorValue(start, -delta, minus_value)) {
00232       double succ_dist = getDoubleDistance(minus_value, goal);
00233       if(succ_dist < start_dist) {
00234         return true;
00235       }
00236     }
00237     return false;
00238   }
00239 
00240   double getDoubleDistance(double start,
00241                            double end)
00242   {
00243     if(joint_limit_.has_position_limits) {
00244       return fabs(end-start);
00245     } else {
00246       return fabs(planning_models::shortestAngularDistance(start,end));
00247     }
00248   }
00249   int getIntegerDistance(double start,
00250                          double end,
00251                          double delta) {
00252     double val;
00253     if(joint_limit_.has_position_limits) {
00254       val = fabs(end-start)/delta;
00255     } else {
00256       val = fabs(planning_models::shortestAngularDistance(start,end))/delta;
00257     }
00258     if(val-floor(val) > std::numeric_limits<double>::epsilon()) {
00259       return ceil(val);
00260     } else {
00261       return floor(val);
00262     }
00263   }
00264 protected:
00265   const planning_models::RobotModel::JointModel* joint_model_;
00266   moveit_msgs::JointLimits joint_limit_;
00267 };
00268 
00269 class JointMotionPrimitive {
00270 public:
00271   virtual bool generateSuccessorState(const std::vector<double>& start,
00272                                       std::vector<double>& end) = 0;
00273 };
00274 
00275 class SingleJointMotionPrimitive : public JointMotionPrimitive {
00276 public:
00277   SingleJointMotionPrimitive(const boost::shared_ptr<JointMotionWrapper>& joint_motion_wrapper,
00278                              unsigned int ind,
00279                              double delta) :
00280     joint_motion_wrapper_(joint_motion_wrapper),
00281     ind_(ind),
00282     delta_(delta)
00283   {
00284   }
00285 
00286   virtual bool generateSuccessorState(const std::vector<double>& start,
00287                                       std::vector<double>& end) {
00288     end = start;
00289     return joint_motion_wrapper_->getSuccessorValue(start[ind_], delta_, end[ind_]);
00290   }
00291 protected:
00292   boost::shared_ptr<JointMotionWrapper> joint_motion_wrapper_;
00293   unsigned int ind_;
00294   double delta_;
00295 };
00296 
00297 }
00298 
00299 #endif


sbpl_interface
Author(s): Gil Jones
autogenerated on Sun Jan 17 2016 12:57:03