constraints_library.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, Willow Garage, Inc.
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 the Willow Garage 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 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/ompl_interface/constraints_library.h>
00038 #include <moveit/ompl_interface/detail/constrained_sampler.h>
00039 #include <moveit/profiler/profiler.h>
00040 #include <ompl/tools/config/SelfConfig.h>
00041 #include <boost/date_time/posix_time/posix_time.hpp>
00042 #include <boost/filesystem.hpp>
00043 #include <fstream>
00044 
00045 namespace ompl_interface
00046 {
00047 
00048 namespace
00049 {
00050 template<typename T>
00051 void msgToHex(const T& msg, std::string &hex)
00052 {
00053   static const char symbol[] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
00054   const size_t serial_size_arg = ros::serialization::serializationLength(msg);
00055 
00056   boost::shared_array<uint8_t> buffer_arg(new uint8_t[serial_size_arg]);
00057   ros::serialization::OStream stream_arg(buffer_arg.get(), serial_size_arg);
00058   ros::serialization::serialize(stream_arg, msg);
00059   hex.resize(serial_size_arg * 2);
00060   for (std::size_t i = 0 ; i < serial_size_arg ; ++i)
00061   {
00062     hex[i * 2] = symbol[buffer_arg[i]/16];
00063     hex[i * 2 + 1] = symbol[buffer_arg[i]%16];
00064   }
00065 }
00066 
00067 template<typename T>
00068 void hexToMsg(const std::string &hex, T& msg)
00069 {
00070   const size_t serial_size_arg = hex.length() / 2;
00071   boost::shared_array<uint8_t> buffer_arg(new uint8_t[serial_size_arg]);
00072   for (std::size_t i = 0 ; i < serial_size_arg ; ++i)
00073   {
00074     buffer_arg[i] =
00075       (hex[i * 2] <= '9' ? (hex[i * 2] - '0') : (hex[i * 2] - 'A' + 10)) * 16 +
00076       (hex[i * 2 + 1] <= '9' ? (hex[i * 2 + 1] - '0') : (hex[i * 2 + 1] - 'A' + 10));
00077   }
00078   ros::serialization::IStream stream_arg(buffer_arg.get(), serial_size_arg);
00079   ros::serialization::deserialize(stream_arg, msg);
00080 }
00081 }
00082 
00083 class ConstraintApproximationStateSampler : public ob::StateSampler
00084 {
00085 public:
00086 
00087   ConstraintApproximationStateSampler(const ob::StateSpace *space, const ConstraintApproximationStateStorage *state_storage, std::size_t milestones) :
00088     ob::StateSampler(space), state_storage_(state_storage)
00089   {
00090     max_index_ = milestones - 1;
00091     inv_dim_ = space->getDimension() > 0 ? 1.0 / (double)space->getDimension() : 1.0;
00092   }
00093 
00094   virtual void sampleUniform(ob::State *state)
00095   {
00096     space_->copyState(state, state_storage_->getState(rng_.uniformInt(0, max_index_)));
00097   }
00098 
00099   virtual void sampleUniformNear(ob::State *state, const ob::State *near, const double distance)
00100   {
00101     int index = -1;
00102     int tag = near->as<ModelBasedStateSpace::StateType>()->tag;
00103 
00104     if (tag >= 0)
00105     {
00106       const ConstrainedStateMetadata &md = state_storage_->getMetadata(tag);
00107       if (!md.first.empty())
00108       {
00109         std::size_t matt = md.first.size() / 3;
00110         std::size_t att = 0;
00111         do
00112         {
00113           index = md.first[rng_.uniformInt(0, md.first.size() - 1)];
00114         } while (dirty_.find(index) != dirty_.end() && ++att < matt);
00115         if (att >= matt)
00116           index = -1;
00117         else
00118           dirty_.insert(index);
00119       }
00120     }
00121     if (index < 0)
00122       index = rng_.uniformInt(0, max_index_);
00123 
00124     double dist = space_->distance(near, state_storage_->getState(index));
00125 
00126     if (dist > distance)
00127     {
00128       double d = pow(rng_.uniform01(), inv_dim_) * distance;
00129       space_->interpolate(near, state_storage_->getState(index), d / dist, state);
00130     }
00131     else
00132       space_->copyState(state, state_storage_->getState(index));
00133   }
00134 
00135   virtual void sampleGaussian(ob::State *state, const ob::State *mean, const double stdDev)
00136   {
00137     sampleUniformNear(state, mean, rng_.gaussian(0.0, stdDev));
00138   }
00139 
00140 protected:
00141 
00143   const ConstraintApproximationStateStorage *state_storage_;
00144   std::set<std::size_t> dirty_;
00145   unsigned int max_index_;
00146   double inv_dim_;
00147 };
00148 
00149 bool interpolateUsingStoredStates(const ConstraintApproximationStateStorage *state_storage, const ob::State *from, const ob::State *to, const double t, ob::State *state)
00150 {
00151   int tag_from = from->as<ModelBasedStateSpace::StateType>()->tag;
00152   int tag_to = to->as<ModelBasedStateSpace::StateType>()->tag;
00153 
00154   if (tag_from < 0 || tag_to < 0)
00155     return false;
00156 
00157   if (tag_from == tag_to)
00158     state_storage->getStateSpace()->copyState(state, to);
00159   else
00160   {
00161     const ConstrainedStateMetadata &md = state_storage->getMetadata(tag_from);
00162 
00163     std::map<std::size_t, std::pair<std::size_t, std::size_t> >::const_iterator it = md.second.find(tag_to);
00164     if (it == md.second.end())
00165       return false;
00166     const std::pair<std::size_t, std::size_t> &istates = it->second;
00167     std::size_t index = (std::size_t)((istates.second - istates.first + 2) * t + 0.5);
00168 
00169     if (index == 0)
00170       state_storage->getStateSpace()->copyState(state, from);
00171     else
00172     {
00173       --index;
00174       if (index >= istates.second - istates.first)
00175         state_storage->getStateSpace()->copyState(state, to);
00176       else
00177         state_storage->getStateSpace()->copyState(state, state_storage->getState(istates.first + index));
00178     }
00179   }
00180   return true;
00181 }
00182 
00183 ompl_interface::InterpolationFunction ompl_interface::ConstraintApproximation::getInterpolationFunction() const
00184 {
00185   if (explicit_motions_ && milestones_ > 0 && milestones_ < state_storage_->size())
00186     return boost::bind(&interpolateUsingStoredStates, state_storage_, _1, _2, _3, _4);
00187   return InterpolationFunction();
00188 }
00189 
00190 ompl::base::StateSamplerPtr allocConstraintApproximationStateSampler(const ob::StateSpace *space, const std::vector<int> &expected_signature,
00191                                                                      const ConstraintApproximationStateStorage *state_storage, std::size_t milestones)
00192 {
00193   std::vector<int> sig;
00194   space->computeSignature(sig);
00195   if (sig != expected_signature)
00196     return ompl::base::StateSamplerPtr();
00197   else
00198     return ompl::base::StateSamplerPtr(new ConstraintApproximationStateSampler(space, state_storage, milestones));
00199 }
00200 
00201 }
00202 
00203 ompl_interface::ConstraintApproximation::ConstraintApproximation(const std::string &group, const std::string &state_space_parameterization,
00204                                                                  bool explicit_motions, const moveit_msgs::Constraints &msg, const std::string &filename,
00205                                                                  const ompl::base::StateStoragePtr &storage, std::size_t milestones) :
00206   group_(group), state_space_parameterization_(state_space_parameterization), explicit_motions_(explicit_motions), constraint_msg_(msg),
00207   ompldb_filename_(filename), state_storage_ptr_(storage), milestones_(milestones)
00208 {
00209   state_storage_ = static_cast<ConstraintApproximationStateStorage*>(state_storage_ptr_.get());
00210   state_storage_->getStateSpace()->computeSignature(space_signature_);
00211   if (milestones_ == 0)
00212     milestones_ = state_storage_->size();
00213 }
00214 
00215 ompl::base::StateSamplerAllocator ompl_interface::ConstraintApproximation::getStateSamplerAllocator(const moveit_msgs::Constraints &msg) const
00216 {
00217   if (state_storage_->size() == 0)
00218     return ompl::base::StateSamplerAllocator();
00219   return boost::bind(&allocConstraintApproximationStateSampler, _1, space_signature_, state_storage_, milestones_);
00220 }
00221 /*
00222 void ompl_interface::ConstraintApproximation::visualizeDistribution(const std::string &link_name, unsigned int count, visualization_msgs::MarkerArray &arr) const
00223 {
00224   robot_state::RobotState kstate(kmodel_);
00225   kstate.setToDefaultValues();
00226 
00227   ompl::RNG rng;
00228   std_msgs::ColorRGBA color;
00229   color.r = 0.0f;
00230   color.g = 1.0f;
00231   color.b = 1.0f;
00232   color.a = 1.0f;
00233   if (state_storage_->size() < count)
00234     count = state_storage_->size();
00235 
00236   for (std::size_t i = 0 ; i < count ; ++i)
00237   {
00238     state_storage_->getStateSpace()->as<ModelBasedStateSpace>()->copyToRobotState(kstate, state_storage_->getState(rng.uniformInt(0, state_storage_->size() - 1)));
00239     const Eigen::Vector3d &pos = kstate.getLinkState(link_name)->getGlobalLinkTransform().translation();
00240 
00241     visualization_msgs::Marker mk;
00242     mk.header.stamp = ros::Time::now();
00243     mk.header.frame_id = kmodel_->getModelFrame();
00244     mk.ns = "stored_constraint_data";
00245     mk.id = i;
00246     mk.type = visualization_msgs::Marker::SPHERE;
00247     mk.action = visualization_msgs::Marker::ADD;
00248     mk.pose.position.x = pos.x();
00249     mk.pose.position.y = pos.y();
00250     mk.pose.position.z = pos.z();
00251     mk.pose.orientation.w = 1.0;
00252     mk.scale.x = mk.scale.y = mk.scale.z = 0.035;
00253     mk.color = color;
00254     mk.lifetime = ros::Duration(30.0);
00255     arr.markers.push_back(mk);
00256   }
00257   }
00258 */
00259 
00260 void ompl_interface::ConstraintsLibrary::loadConstraintApproximations(const std::string &path)
00261 {
00262   constraint_approximations_.clear();
00263   std::ifstream fin((path + "/manifest").c_str());
00264   if (!fin.good())
00265   {
00266     logWarn("Manifest not found in folder '%s'. Not loading constraint approximations.", path.c_str());
00267     return;
00268   }
00269 
00270   logInform("Loading constrained space approximations from '%s'...", path.c_str());
00271 
00272   while (fin.good() && !fin.eof())
00273   {
00274     std::string group, state_space_parameterization, serialization, filename;
00275     bool explicit_motions;
00276     unsigned int milestones;
00277     fin >> group;
00278     if (fin.eof())
00279       break;
00280     fin >> state_space_parameterization;
00281     if (fin.eof())
00282       break;
00283     fin >> explicit_motions;
00284     if (fin.eof())
00285       break;
00286     fin >> milestones;
00287     if (fin.eof())
00288       break;
00289     fin >> serialization;
00290     if (fin.eof())
00291       break;
00292     fin >> filename;
00293     logInform("Loading constraint approximation of type '%s' for group '%s' from '%s'...", state_space_parameterization.c_str(), group.c_str(), filename.c_str());
00294     const ModelBasedPlanningContextPtr &pc = context_manager_.getPlanningContext(group, state_space_parameterization);
00295     if (pc)
00296     {
00297       moveit_msgs::Constraints msg;
00298       hexToMsg(serialization, msg);
00299       ConstraintApproximationStateStorage *cass = new ConstraintApproximationStateStorage(pc->getOMPLSimpleSetup().getStateSpace());
00300       cass->load((path + "/" + filename).c_str());
00301       ConstraintApproximationPtr cap(new ConstraintApproximation(group, state_space_parameterization, explicit_motions, msg, filename,
00302                                                                  ompl::base::StateStoragePtr(cass), milestones));
00303       if (constraint_approximations_.find(cap->getName()) != constraint_approximations_.end())
00304         logWarn("Overwriting constraint approximation named '%s'", cap->getName().c_str());
00305       constraint_approximations_[cap->getName()] = cap;
00306       std::size_t sum = 0;
00307       for (std::size_t i = 0 ; i < cass->size() ; ++i)
00308         sum += cass->getMetadata(i).first.size();
00309       logInform("Loaded %lu states (%lu milestones) and %lu connections (%0.1lf per state) for constraint named '%s'%s",
00310                 cass->size(), cap->getMilestoneCount(), sum, (double)sum / (double)cap->getMilestoneCount(), msg.name.c_str(), explicit_motions ? ". Explicit motions included." : "");
00311     }
00312   }
00313   logInform("Done loading constrained space approximations.");
00314 }
00315 
00316 void ompl_interface::ConstraintsLibrary::saveConstraintApproximations(const std::string &path)
00317 {
00318   logInform("Saving %u constrained space approximations to '%s'", (unsigned int)constraint_approximations_.size(), path.c_str());
00319   try
00320   {
00321     boost::filesystem::create_directories(path);
00322   }
00323   catch(...)
00324   {
00325   }
00326 
00327   std::ofstream fout((path + "/manifest").c_str());
00328   if (fout.good())
00329     for (std::map<std::string, ConstraintApproximationPtr>::const_iterator it = constraint_approximations_.begin() ; it != constraint_approximations_.end() ; ++it)
00330     {
00331       fout << it->second->getGroup() << std::endl;
00332       fout << it->second->getStateSpaceParameterization() << std::endl;
00333       fout << it->second->hasExplicitMotions() << std::endl;
00334       fout << it->second->getMilestoneCount() << std::endl;
00335       std::string serialization;
00336       msgToHex(it->second->getConstraintsMsg(), serialization);
00337       fout << serialization << std::endl;
00338       fout << it->second->getFilename() << std::endl;
00339       if (it->second->getStateStorage())
00340         it->second->getStateStorage()->store((path + "/" + it->second->getFilename()).c_str());
00341     }
00342   else
00343     logError("Unable to save constraint approximation to '%s'", path.c_str());
00344   fout.close();
00345 }
00346 
00347 void ompl_interface::ConstraintsLibrary::clearConstraintApproximations()
00348 {
00349   constraint_approximations_.clear();
00350 }
00351 
00352 void ompl_interface::ConstraintsLibrary::printConstraintApproximations(std::ostream &out) const
00353 {
00354   for (std::map<std::string, ConstraintApproximationPtr>::const_iterator it = constraint_approximations_.begin() ; it != constraint_approximations_.end() ; ++it)
00355   {
00356     out << it->second->getGroup() << std::endl;
00357     out << it->second->getStateSpaceParameterization() << std::endl;
00358     out << it->second->hasExplicitMotions() << std::endl;
00359     out << it->second->getMilestoneCount() << std::endl;
00360     out << it->second->getFilename() << std::endl;
00361     out << it->second->getConstraintsMsg() << std::endl;
00362   }
00363 }
00364 
00365 const ompl_interface::ConstraintApproximationPtr& ompl_interface::ConstraintsLibrary::getConstraintApproximation(const moveit_msgs::Constraints &msg) const
00366 {
00367   std::map<std::string, ConstraintApproximationPtr>::const_iterator it = constraint_approximations_.find(msg.name);
00368   if (it != constraint_approximations_.end())
00369     return it->second;
00370 
00371   static ConstraintApproximationPtr empty;
00372   return empty;
00373 }
00374 
00375 ompl_interface::ConstraintApproximationConstructionResults
00376 ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs::Constraints &constr, const std::string &group,
00377                                                                const planning_scene::PlanningSceneConstPtr &scene,
00378                                                                const ConstraintApproximationConstructionOptions &options)
00379 {
00380   return addConstraintApproximation(constr, constr, group, scene, options);
00381 }
00382 
00383 ompl_interface::ConstraintApproximationConstructionResults
00384 ompl_interface::ConstraintsLibrary::addConstraintApproximation(const moveit_msgs::Constraints &constr_sampling, const moveit_msgs::Constraints &constr_hard,
00385                                                                const std::string &group, const planning_scene::PlanningSceneConstPtr &scene,
00386                                                                const ConstraintApproximationConstructionOptions &options)
00387 {
00388   ConstraintApproximationConstructionResults res;
00389   ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, options.state_space_parameterization);
00390   if (pc)
00391   {
00392     pc->clear();
00393     pc->setPlanningScene(scene);
00394     pc->setCompleteInitialState(scene->getCurrentState());
00395 
00396     ros::WallTime start = ros::WallTime::now();
00397     ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, options, res);
00398     logInform("Spent %lf seconds constructing the database", (ros::WallTime::now() - start).toSec());
00399     if (ss)
00400     {
00401       ConstraintApproximationPtr ca(new ConstraintApproximation(group, options.state_space_parameterization, options.explicit_motions, constr_hard, group + "_" +
00402                                                                 boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) + ".ompldb",
00403                                                                 ss, res.milestones));
00404       if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end())
00405         logWarn("Overwriting constraint approximation named '%s'", ca->getName().c_str());
00406       constraint_approximations_[ca->getName()] = ca;
00407       res.approx = ca;
00408     }
00409     else
00410       logError("Unable to construct constraint approximation for group '%s'", group.c_str());
00411   }
00412   return res;
00413 }
00414 
00415 ompl::base::StateStoragePtr ompl_interface::ConstraintsLibrary::constructConstraintApproximation(const ModelBasedPlanningContextPtr &pcontext,
00416                                                                                                  const moveit_msgs::Constraints &constr_sampling, const moveit_msgs::Constraints &constr_hard,
00417                                                                                                  const ConstraintApproximationConstructionOptions &options,
00418                                                                                                  ConstraintApproximationConstructionResults &result)
00419 {
00420   // state storage structure
00421   ConstraintApproximationStateStorage *cass = new ConstraintApproximationStateStorage(pcontext->getOMPLStateSpace());
00422   ob::StateStoragePtr sstor(cass);
00423 
00424   // construct a sampler for the sampling constraints
00425   kinematic_constraints::KinematicConstraintSet kset(pcontext->getRobotModel());
00426   robot_state::Transforms no_transforms(pcontext->getRobotModel()->getModelFrame());
00427   kset.add(constr_hard, no_transforms);
00428 
00429   const robot_state::RobotState &default_state = pcontext->getCompleteInitialRobotState();
00430 
00431   unsigned int attempts = 0;
00432 
00433   double bounds_val = std::numeric_limits<double>::max() / 2.0 - 1.0;
00434   pcontext->getOMPLStateSpace()->setPlanningVolume(-bounds_val, bounds_val, -bounds_val, bounds_val, -bounds_val, bounds_val);
00435   pcontext->getOMPLStateSpace()->setup();
00436 
00437   // construct the constrained states
00438 
00439   robot_state::RobotState kstate(default_state);
00440   const constraint_samplers::ConstraintSamplerManagerPtr &csmng = pcontext->getConstraintSamplerManager();
00441   ConstrainedSampler *csmp = NULL;
00442   if (csmng)
00443   {
00444     constraint_samplers::ConstraintSamplerPtr cs = csmng->selectSampler(pcontext->getPlanningScene(), pcontext->getJointModelGroup()->getName(), constr_sampling);
00445     if (cs)
00446       csmp = new ConstrainedSampler(pcontext.get(), cs);
00447   }
00448 
00449   ob::StateSamplerPtr ss(csmp ? ob::StateSamplerPtr(csmp) : pcontext->getOMPLStateSpace()->allocDefaultStateSampler());
00450 
00451   ompl::base::ScopedState<> temp(pcontext->getOMPLStateSpace());
00452   int done = -1;
00453   bool slow_warn = false;
00454   ompl::time::point start = ompl::time::now();
00455   while (sstor->size() < options.samples)
00456   {
00457     ++attempts;
00458     int done_now = 100 * sstor->size() / options.samples;
00459     if (done != done_now)
00460     {
00461       done = done_now;
00462       logInform("%d%% complete (kept %0.1lf%% sampled states)", done, 100.0 * (double)sstor->size() / (double)attempts);
00463     }
00464 
00465     if (!slow_warn && attempts > 10 && attempts > sstor->size() * 100)
00466     {
00467       slow_warn = true;
00468       logWarn("Computation of valid state database is very slow...");
00469     }
00470 
00471     if (attempts > options.samples && sstor->size() == 0)
00472     {
00473       logError("Unable to generate any samples");
00474       break;
00475     }
00476 
00477     ss->sampleUniform(temp.get());
00478     pcontext->getOMPLStateSpace()->copyToRobotState(kstate, temp.get());
00479     if (kset.decide(kstate).satisfied)
00480     {
00481       if (sstor->size() < options.samples)
00482       {
00483         temp->as<ModelBasedStateSpace::StateType>()->tag = sstor->size();
00484         sstor->addState(temp.get());
00485       }
00486     }
00487   }
00488 
00489   result.state_sampling_time = ompl::time::seconds(ompl::time::now() - start);
00490   logInform("Generated %u states in %lf seconds", (unsigned int)sstor->size(), result.state_sampling_time);
00491   if (csmp)
00492   {
00493     result.sampling_success_rate = csmp->getConstrainedSamplingRate();
00494     logInform("Constrained sampling rate: %lf", result.sampling_success_rate);
00495   }
00496 
00497   result.milestones = sstor->size();
00498   if (options.edges_per_sample > 0)
00499   {
00500     logInform("Computing graph connections (max %u edges per sample) ...", options.edges_per_sample);
00501 
00502     // construct connexions
00503     const ob::StateSpacePtr &space = pcontext->getOMPLSimpleSetup().getStateSpace();
00504     unsigned int milestones = sstor->size();
00505     std::vector<ob::State*> int_states(options.max_explicit_points, NULL);
00506     pcontext->getOMPLSimpleSetup().getSpaceInformation()->allocStates(int_states);
00507     robot_state::JointStateGroup *jsg = kstate.getJointStateGroup(pcontext->getJointModelGroup()->getName());
00508 
00509     ompl::time::point start = ompl::time::now();
00510     int good = 0;
00511     int done = -1;
00512 
00513     for (std::size_t j = 0 ; j < milestones ; ++j)
00514     {
00515       int done_now = 100 * j / milestones;
00516       if (done != done_now)
00517       {
00518     done = done_now;
00519         logInform("%d%% complete", done);
00520       }
00521       if (cass->getMetadata(j).first.size() >= options.edges_per_sample)
00522         continue;
00523 
00524       const ob::State *sj = sstor->getState(j);
00525 
00526       for (std::size_t i = j + 1 ; i < milestones ; ++i)
00527       {
00528         if (cass->getMetadata(i).first.size() >= options.edges_per_sample)
00529           continue;
00530     double d = space->distance(sstor->getState(i), sj);
00531         if (d >= options.max_edge_length)
00532           continue;
00533         unsigned int isteps = std::min<unsigned int>(options.max_explicit_points, d / options.explicit_points_resolution);
00534         double step = 1.0 / (double)isteps;
00535         double remain = 1.0;
00536         bool ok = true;
00537         space->interpolate(sstor->getState(i), sj, step, int_states[0]);
00538         for (unsigned int k = 1 ; k < isteps ; ++k)
00539         {
00540           double this_step = step / (1.0 - (k - 1) * step);
00541           space->interpolate(int_states[k-1], sj, this_step, int_states[k]);
00542       pcontext->getOMPLStateSpace()->copyToRobotState(kstate, int_states[k]);
00543       if (!kset.decide(kstate).satisfied)
00544           {
00545             ok = false;
00546             break;
00547           }
00548         }
00549 
00550         if (ok)
00551         {
00552           cass->getMetadata(i).first.push_back(j);
00553           cass->getMetadata(j).first.push_back(i);
00554 
00555           if (options.explicit_motions)
00556           {
00557             cass->getMetadata(i).second[j].first = sstor->size();
00558             for (unsigned int k = 0 ; k < isteps ; ++k)
00559             {
00560               int_states[k]->as<ModelBasedStateSpace::StateType>()->tag = -1;
00561               sstor->addState(int_states[k]);
00562             }
00563             cass->getMetadata(i).second[j].second = sstor->size();
00564             cass->getMetadata(j).second[i] = cass->getMetadata(i).second[j];
00565           }
00566 
00567           good++;
00568           if (cass->getMetadata(j).first.size() >= options.edges_per_sample)
00569             break;
00570         }
00571       }
00572     }
00573 
00574     result.state_connection_time = ompl::time::seconds(ompl::time::now() - start);
00575     logInform("Computed possible connexions in %lf seconds. Added %d connexions", result.state_connection_time, good);
00576     pcontext->getOMPLSimpleSetup().getSpaceInformation()->freeStates(int_states);
00577 
00578     return sstor;
00579   }
00580 }


ompl
Author(s): Ioan Sucan
autogenerated on Mon Oct 6 2014 11:12:03