constraints_library.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
40 #include <ompl/tools/config/SelfConfig.h>
41 #include <boost/date_time/posix_time/posix_time.hpp>
42 #include <boost/filesystem.hpp>
43 #include <fstream>
44 
45 namespace ompl_interface
46 {
47 namespace
48 {
49 template <typename T>
50 void msgToHex(const T& msg, std::string& hex)
51 {
52  static const char symbol[] = { '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' };
53  const size_t serial_size_arg = ros::serialization::serializationLength(msg);
54 
55  boost::shared_array<uint8_t> buffer_arg(new uint8_t[serial_size_arg]);
56  ros::serialization::OStream stream_arg(buffer_arg.get(), serial_size_arg);
57  ros::serialization::serialize(stream_arg, msg);
58  hex.resize(serial_size_arg * 2);
59  for (std::size_t i = 0; i < serial_size_arg; ++i)
60  {
61  hex[i * 2] = symbol[buffer_arg[i] / 16];
62  hex[i * 2 + 1] = symbol[buffer_arg[i] % 16];
63  }
64 }
65 
66 template <typename T>
67 void hexToMsg(const std::string& hex, T& msg)
68 {
69  const size_t serial_size_arg = hex.length() / 2;
70  boost::shared_array<uint8_t> buffer_arg(new uint8_t[serial_size_arg]);
71  for (std::size_t i = 0; i < serial_size_arg; ++i)
72  {
73  buffer_arg[i] = (hex[i * 2] <= '9' ? (hex[i * 2] - '0') : (hex[i * 2] - 'A' + 10)) * 16 +
74  (hex[i * 2 + 1] <= '9' ? (hex[i * 2 + 1] - '0') : (hex[i * 2 + 1] - 'A' + 10));
75  }
76  ros::serialization::IStream stream_arg(buffer_arg.get(), serial_size_arg);
77  ros::serialization::deserialize(stream_arg, msg);
78 }
79 }
80 
81 class ConstraintApproximationStateSampler : public ob::StateSampler
82 {
83 public:
84  ConstraintApproximationStateSampler(const ob::StateSpace* space,
85  const ConstraintApproximationStateStorage* state_storage, std::size_t milestones)
86  : ob::StateSampler(space), state_storage_(state_storage)
87  {
88  max_index_ = milestones - 1;
89  inv_dim_ = space->getDimension() > 0 ? 1.0 / (double)space->getDimension() : 1.0;
90  }
91 
92  virtual void sampleUniform(ob::State* state)
93  {
94  space_->copyState(state, state_storage_->getState(rng_.uniformInt(0, max_index_)));
95  }
96 
97  virtual void sampleUniformNear(ob::State* state, const ob::State* near, const double distance)
98  {
99  int index = -1;
100  int tag = near->as<ModelBasedStateSpace::StateType>()->tag;
101 
102  if (tag >= 0)
103  {
104  const ConstrainedStateMetadata& md = state_storage_->getMetadata(tag);
105  if (!md.first.empty())
106  {
107  std::size_t matt = md.first.size() / 3;
108  std::size_t att = 0;
109  do
110  {
111  index = md.first[rng_.uniformInt(0, md.first.size() - 1)];
112  } while (dirty_.find(index) != dirty_.end() && ++att < matt);
113  if (att >= matt)
114  index = -1;
115  else
116  dirty_.insert(index);
117  }
118  }
119  if (index < 0)
120  index = rng_.uniformInt(0, max_index_);
121 
122  double dist = space_->distance(near, state_storage_->getState(index));
123 
124  if (dist > distance)
125  {
126  double d = pow(rng_.uniform01(), inv_dim_) * distance;
127  space_->interpolate(near, state_storage_->getState(index), d / dist, state);
128  }
129  else
130  space_->copyState(state, state_storage_->getState(index));
131  }
132 
133  virtual void sampleGaussian(ob::State* state, const ob::State* mean, const double stdDev)
134  {
135  sampleUniformNear(state, mean, rng_.gaussian(0.0, stdDev));
136  }
137 
138 protected:
141  std::set<std::size_t> dirty_;
142  unsigned int max_index_;
143  double inv_dim_;
144 };
145 
146 bool interpolateUsingStoredStates(const ConstraintApproximationStateStorage* state_storage, const ob::State* from,
147  const ob::State* to, const double t, ob::State* state)
148 {
149  int tag_from = from->as<ModelBasedStateSpace::StateType>()->tag;
150  int tag_to = to->as<ModelBasedStateSpace::StateType>()->tag;
151 
152  if (tag_from < 0 || tag_to < 0)
153  return false;
154 
155  if (tag_from == tag_to)
156  state_storage->getStateSpace()->copyState(state, to);
157  else
158  {
159  const ConstrainedStateMetadata& md = state_storage->getMetadata(tag_from);
160 
161  std::map<std::size_t, std::pair<std::size_t, std::size_t> >::const_iterator it = md.second.find(tag_to);
162  if (it == md.second.end())
163  return false;
164  const std::pair<std::size_t, std::size_t>& istates = it->second;
165  std::size_t index = (std::size_t)((istates.second - istates.first + 2) * t + 0.5);
166 
167  if (index == 0)
168  state_storage->getStateSpace()->copyState(state, from);
169  else
170  {
171  --index;
172  if (index >= istates.second - istates.first)
173  state_storage->getStateSpace()->copyState(state, to);
174  else
175  state_storage->getStateSpace()->copyState(state, state_storage->getState(istates.first + index));
176  }
177  }
178  return true;
179 }
180 
182 {
183  if (explicit_motions_ && milestones_ > 0 && milestones_ < state_storage_->size())
184  return boost::bind(&interpolateUsingStoredStates, state_storage_, _1, _2, _3, _4);
185  return InterpolationFunction();
186 }
187 
188 ompl::base::StateSamplerPtr
189 allocConstraintApproximationStateSampler(const ob::StateSpace* space, const std::vector<int>& expected_signature,
190  const ConstraintApproximationStateStorage* state_storage,
191  std::size_t milestones)
192 {
193  std::vector<int> sig;
194  space->computeSignature(sig);
195  if (sig != expected_signature)
196  return ompl::base::StateSamplerPtr();
197  else
198  return ompl::base::StateSamplerPtr(new ConstraintApproximationStateSampler(space, state_storage, milestones));
199 }
200 }
201 
203  const std::string& group, const std::string& state_space_parameterization, bool explicit_motions,
204  const moveit_msgs::Constraints& msg, const std::string& filename, const ompl::base::StateStoragePtr& storage,
205  std::size_t milestones)
206  : group_(group)
207  , state_space_parameterization_(state_space_parameterization)
208  , explicit_motions_(explicit_motions)
209  , constraint_msg_(msg)
210  , ompldb_filename_(filename)
211  , state_storage_ptr_(storage)
212  , milestones_(milestones)
213 {
215  state_storage_->getStateSpace()->computeSignature(space_signature_);
216  if (milestones_ == 0)
217  milestones_ = state_storage_->size();
218 }
219 
220 ompl::base::StateSamplerAllocator
222 {
223  if (state_storage_->size() == 0)
224  return ompl::base::StateSamplerAllocator();
226 }
227 /*
228 void ompl_interface::ConstraintApproximation::visualizeDistribution(const std::string &link_name, unsigned int count,
229 visualization_msgs::MarkerArray &arr) const
230 {
231  robot_state::RobotState kstate(kmodel_);
232  kstate.setToDefaultValues();
233 
234  ompl::RNG rng;
235  std_msgs::ColorRGBA color;
236  color.r = 0.0f;
237  color.g = 1.0f;
238  color.b = 1.0f;
239  color.a = 1.0f;
240  if (state_storage_->size() < count)
241  count = state_storage_->size();
242 
243  for (std::size_t i = 0 ; i < count ; ++i)
244  {
245  state_storage_->getStateSpace()->as<ModelBasedStateSpace>()->copyToRobotState(kstate,
246 state_storage_->getState(rng.uniformInt(0, state_storage_->size() - 1)));
247  const Eigen::Vector3d &pos = kstate.getLinkState(link_name)->getGlobalLinkTransform().translation();
248 
249  visualization_msgs::Marker mk;
250  mk.header.stamp = ros::Time::now();
251  mk.header.frame_id = kmodel_->getModelFrame();
252  mk.ns = "stored_constraint_data";
253  mk.id = i;
254  mk.type = visualization_msgs::Marker::SPHERE;
255  mk.action = visualization_msgs::Marker::ADD;
256  mk.pose.position.x = pos.x();
257  mk.pose.position.y = pos.y();
258  mk.pose.position.z = pos.z();
259  mk.pose.orientation.w = 1.0;
260  mk.scale.x = mk.scale.y = mk.scale.z = 0.035;
261  mk.color = color;
262  mk.lifetime = ros::Duration(30.0);
263  arr.markers.push_back(mk);
264  }
265  }
266 */
267 
269 {
270  constraint_approximations_.clear();
271  std::ifstream fin((path + "/manifest").c_str());
272  if (!fin.good())
273  {
274  ROS_WARN_NAMED("constraints_library", "Manifest not found in folder '%s'. Not loading constraint approximations.",
275  path.c_str());
276  return;
277  }
278 
279  ROS_INFO_NAMED("constraints_library", "Loading constrained space approximations from '%s'...", path.c_str());
280 
281  while (fin.good() && !fin.eof())
282  {
283  std::string group, state_space_parameterization, serialization, filename;
284  bool explicit_motions;
285  unsigned int milestones;
286  fin >> group;
287  if (fin.eof())
288  break;
289  fin >> state_space_parameterization;
290  if (fin.eof())
291  break;
292  fin >> explicit_motions;
293  if (fin.eof())
294  break;
295  fin >> milestones;
296  if (fin.eof())
297  break;
298  fin >> serialization;
299  if (fin.eof())
300  break;
301  fin >> filename;
302  ROS_INFO_NAMED("constraints_library", "Loading constraint approximation of type '%s' for group '%s' from '%s'...",
303  state_space_parameterization.c_str(), group.c_str(), filename.c_str());
304  const ModelBasedPlanningContextPtr& pc = context_manager_.getPlanningContext(group, state_space_parameterization);
305  if (pc)
306  {
307  moveit_msgs::Constraints msg;
308  hexToMsg(serialization, msg);
310  new ConstraintApproximationStateStorage(pc->getOMPLSimpleSetup()->getStateSpace());
311  cass->load((path + "/" + filename).c_str());
312  ConstraintApproximationPtr cap(new ConstraintApproximation(group, state_space_parameterization, explicit_motions,
313  msg, filename, ompl::base::StateStoragePtr(cass),
314  milestones));
315  if (constraint_approximations_.find(cap->getName()) != constraint_approximations_.end())
316  ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'",
317  cap->getName().c_str());
318  constraint_approximations_[cap->getName()] = cap;
319  std::size_t sum = 0;
320  for (std::size_t i = 0; i < cass->size(); ++i)
321  sum += cass->getMetadata(i).first.size();
322  ROS_INFO_NAMED("constraints_library", "Loaded %lu states (%lu milestones) and %lu connections (%0.1lf per state) "
323  "for constraint named '%s'%s",
324  cass->size(), cap->getMilestoneCount(), sum, (double)sum / (double)cap->getMilestoneCount(),
325  msg.name.c_str(), explicit_motions ? ". Explicit motions included." : "");
326  }
327  }
328  ROS_INFO_NAMED("constraints_library", "Done loading constrained space approximations.");
329 }
330 
332 {
333  ROS_INFO_NAMED("constraints_library", "Saving %u constrained space approximations to '%s'",
334  (unsigned int)constraint_approximations_.size(), path.c_str());
335  try
336  {
337  boost::filesystem::create_directories(path);
338  }
339  catch (...)
340  {
341  }
342 
343  std::ofstream fout((path + "/manifest").c_str());
344  if (fout.good())
345  for (std::map<std::string, ConstraintApproximationPtr>::const_iterator it = constraint_approximations_.begin();
346  it != constraint_approximations_.end(); ++it)
347  {
348  fout << it->second->getGroup() << std::endl;
349  fout << it->second->getStateSpaceParameterization() << std::endl;
350  fout << it->second->hasExplicitMotions() << std::endl;
351  fout << it->second->getMilestoneCount() << std::endl;
352  std::string serialization;
353  msgToHex(it->second->getConstraintsMsg(), serialization);
354  fout << serialization << std::endl;
355  fout << it->second->getFilename() << std::endl;
356  if (it->second->getStateStorage())
357  it->second->getStateStorage()->store((path + "/" + it->second->getFilename()).c_str());
358  }
359  else
360  ROS_ERROR_NAMED("constraints_library", "Unable to save constraint approximation to '%s'", path.c_str());
361  fout.close();
362 }
363 
365 {
366  constraint_approximations_.clear();
367 }
368 
370 {
371  for (std::map<std::string, ConstraintApproximationPtr>::const_iterator it = constraint_approximations_.begin();
372  it != constraint_approximations_.end(); ++it)
373  {
374  out << it->second->getGroup() << std::endl;
375  out << it->second->getStateSpaceParameterization() << std::endl;
376  out << it->second->hasExplicitMotions() << std::endl;
377  out << it->second->getMilestoneCount() << std::endl;
378  out << it->second->getFilename() << std::endl;
379  out << it->second->getConstraintsMsg() << std::endl;
380  }
381 }
382 
383 const ompl_interface::ConstraintApproximationPtr&
384 ompl_interface::ConstraintsLibrary::getConstraintApproximation(const moveit_msgs::Constraints& msg) const
385 {
386  std::map<std::string, ConstraintApproximationPtr>::const_iterator it = constraint_approximations_.find(msg.name);
387  if (it != constraint_approximations_.end())
388  return it->second;
389 
390  static ConstraintApproximationPtr empty;
391  return empty;
392 }
393 
396  const moveit_msgs::Constraints& constr, const std::string& group,
397  const planning_scene::PlanningSceneConstPtr& scene, const ConstraintApproximationConstructionOptions& options)
398 {
399  return addConstraintApproximation(constr, constr, group, scene, options);
400 }
401 
404  const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard,
405  const std::string& group, const planning_scene::PlanningSceneConstPtr& scene,
407 {
409  ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, options.state_space_parameterization);
410  if (pc)
411  {
412  pc->clear();
413  pc->setPlanningScene(scene);
414  pc->setCompleteInitialState(scene->getCurrentState());
415 
417  ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, options, res);
418  ROS_INFO_NAMED("constraints_library", "Spent %lf seconds constructing the database",
419  (ros::WallTime::now() - start).toSec());
420  if (ss)
421  {
422  ConstraintApproximationPtr ca(new ConstraintApproximation(
423  group, options.state_space_parameterization, options.explicit_motions, constr_hard,
424  group + "_" + boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) +
425  ".ompldb",
426  ss, res.milestones));
427  if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end())
428  ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", ca->getName().c_str());
429  constraint_approximations_[ca->getName()] = ca;
430  res.approx = ca;
431  }
432  else
433  ROS_ERROR_NAMED("constraints_library", "Unable to construct constraint approximation for group '%s'",
434  group.c_str());
435  }
436  return res;
437 }
438 
440  const ModelBasedPlanningContextPtr& pcontext, const moveit_msgs::Constraints& constr_sampling,
441  const moveit_msgs::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options,
443 {
444  // state storage structure
445  ConstraintApproximationStateStorage* cass = new ConstraintApproximationStateStorage(pcontext->getOMPLStateSpace());
446  ob::StateStoragePtr sstor(cass);
447 
448  // construct a sampler for the sampling constraints
449  kinematic_constraints::KinematicConstraintSet kset(pcontext->getRobotModel());
450  robot_state::Transforms no_transforms(pcontext->getRobotModel()->getModelFrame());
451  kset.add(constr_hard, no_transforms);
452 
453  const robot_state::RobotState& default_state = pcontext->getCompleteInitialRobotState();
454 
455  unsigned int attempts = 0;
456 
457  double bounds_val = std::numeric_limits<double>::max() / 2.0 - 1.0;
458  pcontext->getOMPLStateSpace()->setPlanningVolume(-bounds_val, bounds_val, -bounds_val, bounds_val, -bounds_val,
459  bounds_val);
460  pcontext->getOMPLStateSpace()->setup();
461 
462  // construct the constrained states
463 
464  robot_state::RobotState kstate(default_state);
465  const constraint_samplers::ConstraintSamplerManagerPtr& csmng = pcontext->getConstraintSamplerManager();
466  ConstrainedSampler* csmp = NULL;
467  if (csmng)
468  {
469  constraint_samplers::ConstraintSamplerPtr cs =
470  csmng->selectSampler(pcontext->getPlanningScene(), pcontext->getJointModelGroup()->getName(), constr_sampling);
471  if (cs)
472  csmp = new ConstrainedSampler(pcontext.get(), cs);
473  }
474 
475  ob::StateSamplerPtr ss(csmp ? ob::StateSamplerPtr(csmp) : pcontext->getOMPLStateSpace()->allocDefaultStateSampler());
476 
477  ompl::base::ScopedState<> temp(pcontext->getOMPLStateSpace());
478  int done = -1;
479  bool slow_warn = false;
480  ompl::time::point start = ompl::time::now();
481  while (sstor->size() < options.samples)
482  {
483  ++attempts;
484  int done_now = 100 * sstor->size() / options.samples;
485  if (done != done_now)
486  {
487  done = done_now;
488  ROS_INFO_NAMED("constraints_library", "%d%% complete (kept %0.1lf%% sampled states)", done,
489  100.0 * (double)sstor->size() / (double)attempts);
490  }
491 
492  if (!slow_warn && attempts > 10 && attempts > sstor->size() * 100)
493  {
494  slow_warn = true;
495  ROS_WARN_NAMED("constraints_library", "Computation of valid state database is very slow...");
496  }
497 
498  if (attempts > options.samples && sstor->size() == 0)
499  {
500  ROS_ERROR_NAMED("constraints_library", "Unable to generate any samples");
501  break;
502  }
503 
504  ss->sampleUniform(temp.get());
505  pcontext->getOMPLStateSpace()->copyToRobotState(kstate, temp.get());
506  if (kset.decide(kstate).satisfied)
507  {
508  if (sstor->size() < options.samples)
509  {
510  temp->as<ModelBasedStateSpace::StateType>()->tag = sstor->size();
511  sstor->addState(temp.get());
512  }
513  }
514  }
515 
516  result.state_sampling_time = ompl::time::seconds(ompl::time::now() - start);
517  ROS_INFO_NAMED("constraints_library", "Generated %u states in %lf seconds", (unsigned int)sstor->size(),
518  result.state_sampling_time);
519  if (csmp)
520  {
522  ROS_INFO_NAMED("constraints_library", "Constrained sampling rate: %lf", result.sampling_success_rate);
523  }
524 
525  result.milestones = sstor->size();
526  if (options.edges_per_sample > 0)
527  {
528  ROS_INFO_NAMED("constraints_library", "Computing graph connections (max %u edges per sample) ...",
529  options.edges_per_sample);
530 
531  // construct connexions
532  const ob::StateSpacePtr& space = pcontext->getOMPLSimpleSetup()->getStateSpace();
533  unsigned int milestones = sstor->size();
534  std::vector<ob::State*> int_states(options.max_explicit_points, NULL);
535  pcontext->getOMPLSimpleSetup()->getSpaceInformation()->allocStates(int_states);
536 
537  ompl::time::point start = ompl::time::now();
538  int good = 0;
539  int done = -1;
540 
541  for (std::size_t j = 0; j < milestones; ++j)
542  {
543  int done_now = 100 * j / milestones;
544  if (done != done_now)
545  {
546  done = done_now;
547  ROS_INFO_NAMED("constraints_library", "%d%% complete", done);
548  }
549  if (cass->getMetadata(j).first.size() >= options.edges_per_sample)
550  continue;
551 
552  const ob::State* sj = sstor->getState(j);
553 
554  for (std::size_t i = j + 1; i < milestones; ++i)
555  {
556  if (cass->getMetadata(i).first.size() >= options.edges_per_sample)
557  continue;
558  double d = space->distance(sstor->getState(i), sj);
559  if (d >= options.max_edge_length)
560  continue;
561  unsigned int isteps =
562  std::min<unsigned int>(options.max_explicit_points, d / options.explicit_points_resolution);
563  double step = 1.0 / (double)isteps;
564  bool ok = true;
565  space->interpolate(sstor->getState(i), sj, step, int_states[0]);
566  for (unsigned int k = 1; k < isteps; ++k)
567  {
568  double this_step = step / (1.0 - (k - 1) * step);
569  space->interpolate(int_states[k - 1], sj, this_step, int_states[k]);
570  pcontext->getOMPLStateSpace()->copyToRobotState(kstate, int_states[k]);
571  if (!kset.decide(kstate).satisfied)
572  {
573  ok = false;
574  break;
575  }
576  }
577 
578  if (ok)
579  {
580  cass->getMetadata(i).first.push_back(j);
581  cass->getMetadata(j).first.push_back(i);
582 
583  if (options.explicit_motions)
584  {
585  cass->getMetadata(i).second[j].first = sstor->size();
586  for (unsigned int k = 0; k < isteps; ++k)
587  {
588  int_states[k]->as<ModelBasedStateSpace::StateType>()->tag = -1;
589  sstor->addState(int_states[k]);
590  }
591  cass->getMetadata(i).second[j].second = sstor->size();
592  cass->getMetadata(j).second[i] = cass->getMetadata(i).second[j];
593  }
594 
595  good++;
596  if (cass->getMetadata(j).first.size() >= options.edges_per_sample)
597  break;
598  }
599  }
600  }
601 
602  result.state_connection_time = ompl::time::seconds(ompl::time::now() - start);
603  ROS_INFO_NAMED("constraints_library", "Computed possible connexions in %lf seconds. Added %d connexions",
604  result.state_connection_time, good);
605  pcontext->getOMPLSimpleSetup()->getSpaceInformation()->freeStates(int_states);
606 
607  return sstor;
608  }
609 
610  // TODO(davetcoleman): this function did not originally return a value, causing compiler warnings in ROS Melodic
611  // Update with more intelligent logic as needed
612  ROS_ERROR_NAMED("constraints_library", "No StateStoragePtr found - implement better solution here.");
613  return sstor;
614 }
d
std::pair< std::vector< std::size_t >, std::map< std::size_t, std::pair< std::size_t, std::size_t > > > ConstrainedStateMetadata
filename
#define ROS_INFO_NAMED(name,...)
#define ROS_WARN_NAMED(name,...)
virtual void sampleGaussian(ob::State *state, const ob::State *mean, const double stdDev)
ompl::base::StateSamplerPtr allocConstraintApproximationStateSampler(const ob::StateSpace *space, const std::vector< int > &expected_signature, const ConstraintApproximationStateStorage *state_storage, std::size_t milestones)
ConstraintApproximationStateSampler(const ob::StateSpace *space, const ConstraintApproximationStateStorage *state_storage, std::size_t milestones)
The MoveIt! interface to OMPL.
virtual void sampleUniformNear(ob::State *state, const ob::State *near, const double distance)
ompl::base::StateSamplerAllocator getStateSamplerAllocator(const moveit_msgs::Constraints &msg) const
ConstraintApproximationStateStorage * state_storage_
void printConstraintApproximations(std::ostream &out=std::cout) const
bool add(const moveit_msgs::Constraints &c, const robot_state::Transforms &tf)
void saveConstraintApproximations(const std::string &path)
unsigned int index
ompl::base::StateStoragePtr state_storage_ptr_
ConstraintApproximationConstructionResults addConstraintApproximation(const moveit_msgs::Constraints &constr_sampling, const moveit_msgs::Constraints &constr_hard, const std::string &group, const planning_scene::PlanningSceneConstPtr &scene, const ConstraintApproximationConstructionOptions &options)
void serialize(Stream &stream, const T &t)
const ConstraintApproximationPtr & getConstraintApproximation(const moveit_msgs::Constraints &msg) const
const ConstraintApproximationStateStorage * state_storage_
The states to sample from.
ConstraintApproximation(const std::string &group, const std::string &state_space_parameterization, bool explicit_motions, const moveit_msgs::Constraints &msg, const std::string &filename, const ompl::base::StateStoragePtr &storage, std::size_t milestones=0)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
unsigned int step
uint32_t serializationLength(const T &t)
static WallTime now()
void loadConstraintApproximations(const std::string &path)
bool interpolateUsingStoredStates(const ConstraintApproximationStateStorage *state_storage, const ob::State *from, const ob::State *to, const double t, ob::State *state)
InterpolationFunction getInterpolationFunction() const
#define ROS_ERROR_NAMED(name,...)
ompl::base::StateStorageWithMetadata< ConstrainedStateMetadata > ConstraintApproximationStateStorage
ompl::base::StateStoragePtr constructConstraintApproximation(const ModelBasedPlanningContextPtr &pcontext, const moveit_msgs::Constraints &constr_sampling, const moveit_msgs::Constraints &constr_hard, const ConstraintApproximationConstructionOptions &options, ConstraintApproximationConstructionResults &result)
void deserialize(Stream &stream, T &t)
boost::function< bool(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state)> InterpolationFunction


ompl
Author(s): Ioan Sucan
autogenerated on Wed Jul 10 2019 04:03:46