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


ompl
Author(s): Ioan Sucan
autogenerated on Mon Jul 24 2017 02:21:33