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 allocConstraintApproximationStateSampler(
189  const ob::StateSpace* space, const std::vector<int>& expected_signature,
190  const ConstraintApproximationStateStorage* state_storage, std::size_t milestones)
191 {
192  std::vector<int> sig;
193  space->computeSignature(sig);
194  if (sig != expected_signature)
195  return ompl::base::StateSamplerPtr();
196  else
197  return ompl::base::StateSamplerPtr(new ConstraintApproximationStateSampler(space, state_storage, milestones));
198 }
199 }
200 
202  const std::string& group, const std::string& state_space_parameterization, bool explicit_motions,
203  const moveit_msgs::Constraints& msg, const std::string& filename, const ompl::base::StateStoragePtr& storage,
204  std::size_t milestones)
205  : group_(group)
206  , state_space_parameterization_(state_space_parameterization)
207  , explicit_motions_(explicit_motions)
208  , constraint_msg_(msg)
209  , ompldb_filename_(filename)
210  , state_storage_ptr_(storage)
211  , milestones_(milestones)
212 {
214  state_storage_->getStateSpace()->computeSignature(space_signature_);
215  if (milestones_ == 0)
216  milestones_ = state_storage_->size();
217 }
218 
219 ompl::base::StateSamplerAllocator
221 {
222  if (state_storage_->size() == 0)
223  return ompl::base::StateSamplerAllocator();
225 }
226 /*
227 void ompl_interface::ConstraintApproximation::visualizeDistribution(const std::string &link_name, unsigned int count,
228 visualization_msgs::MarkerArray &arr) const
229 {
230  robot_state::RobotState kstate(kmodel_);
231  kstate.setToDefaultValues();
232 
233  ompl::RNG rng;
234  std_msgs::ColorRGBA color;
235  color.r = 0.0f;
236  color.g = 1.0f;
237  color.b = 1.0f;
238  color.a = 1.0f;
239  if (state_storage_->size() < count)
240  count = state_storage_->size();
241 
242  for (std::size_t i = 0 ; i < count ; ++i)
243  {
244  state_storage_->getStateSpace()->as<ModelBasedStateSpace>()->copyToRobotState(kstate,
245 state_storage_->getState(rng.uniformInt(0, state_storage_->size() - 1)));
246  const Eigen::Vector3d &pos = kstate.getLinkState(link_name)->getGlobalLinkTransform().translation();
247 
248  visualization_msgs::Marker mk;
249  mk.header.stamp = ros::Time::now();
250  mk.header.frame_id = kmodel_->getModelFrame();
251  mk.ns = "stored_constraint_data";
252  mk.id = i;
253  mk.type = visualization_msgs::Marker::SPHERE;
254  mk.action = visualization_msgs::Marker::ADD;
255  mk.pose.position.x = pos.x();
256  mk.pose.position.y = pos.y();
257  mk.pose.position.z = pos.z();
258  mk.pose.orientation.w = 1.0;
259  mk.scale.x = mk.scale.y = mk.scale.z = 0.035;
260  mk.color = color;
261  mk.lifetime = ros::Duration(30.0);
262  arr.markers.push_back(mk);
263  }
264  }
265 */
266 
268 {
269  constraint_approximations_.clear();
270  std::ifstream fin((path + "/manifest").c_str());
271  if (!fin.good())
272  {
273  ROS_WARN_NAMED("constraints_library", "Manifest not found in folder '%s'. Not loading constraint approximations.",
274  path.c_str());
275  return;
276  }
277 
278  ROS_INFO_NAMED("constraints_library", "Loading constrained space approximations from '%s'...", path.c_str());
279 
280  while (fin.good() && !fin.eof())
281  {
282  std::string group, state_space_parameterization, serialization, filename;
283  bool explicit_motions;
284  unsigned int milestones;
285  fin >> group;
286  if (fin.eof())
287  break;
288  fin >> state_space_parameterization;
289  if (fin.eof())
290  break;
291  fin >> explicit_motions;
292  if (fin.eof())
293  break;
294  fin >> milestones;
295  if (fin.eof())
296  break;
297  fin >> serialization;
298  if (fin.eof())
299  break;
300  fin >> filename;
301  ROS_INFO_NAMED("constraints_library", "Loading constraint approximation of type '%s' for group '%s' from '%s'...",
302  state_space_parameterization.c_str(), group.c_str(), filename.c_str());
303  const ModelBasedPlanningContextPtr& pc = context_manager_.getPlanningContext(group, state_space_parameterization);
304  if (pc)
305  {
306  moveit_msgs::Constraints msg;
307  hexToMsg(serialization, msg);
309  new ConstraintApproximationStateStorage(pc->getOMPLSimpleSetup()->getStateSpace());
310  cass->load((path + "/" + filename).c_str());
311  ConstraintApproximationPtr cap(new ConstraintApproximation(group, state_space_parameterization, explicit_motions,
312  msg, filename, ompl::base::StateStoragePtr(cass),
313  milestones));
314  if (constraint_approximations_.find(cap->getName()) != constraint_approximations_.end())
315  ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'",
316  cap->getName().c_str());
317  constraint_approximations_[cap->getName()] = cap;
318  std::size_t sum = 0;
319  for (std::size_t i = 0; i < cass->size(); ++i)
320  sum += cass->getMetadata(i).first.size();
321  ROS_INFO_NAMED("constraints_library", "Loaded %lu states (%lu milestones) and %lu connections (%0.1lf per state) "
322  "for constraint named '%s'%s",
323  cass->size(), cap->getMilestoneCount(), sum, (double)sum / (double)cap->getMilestoneCount(),
324  msg.name.c_str(), explicit_motions ? ". Explicit motions included." : "");
325  }
326  }
327  ROS_INFO_NAMED("constraints_library", "Done loading constrained space approximations.");
328 }
329 
331 {
332  ROS_INFO_NAMED("constraints_library", "Saving %u constrained space approximations to '%s'",
333  (unsigned int)constraint_approximations_.size(), path.c_str());
334  try
335  {
336  boost::filesystem::create_directories(path);
337  }
338  catch (...)
339  {
340  }
341 
342  std::ofstream fout((path + "/manifest").c_str());
343  if (fout.good())
344  for (std::map<std::string, ConstraintApproximationPtr>::const_iterator it = constraint_approximations_.begin();
345  it != constraint_approximations_.end(); ++it)
346  {
347  fout << it->second->getGroup() << std::endl;
348  fout << it->second->getStateSpaceParameterization() << std::endl;
349  fout << it->second->hasExplicitMotions() << std::endl;
350  fout << it->second->getMilestoneCount() << std::endl;
351  std::string serialization;
352  msgToHex(it->second->getConstraintsMsg(), serialization);
353  fout << serialization << std::endl;
354  fout << it->second->getFilename() << std::endl;
355  if (it->second->getStateStorage())
356  it->second->getStateStorage()->store((path + "/" + it->second->getFilename()).c_str());
357  }
358  else
359  ROS_ERROR_NAMED("constraints_library", "Unable to save constraint approximation to '%s'", path.c_str());
360  fout.close();
361 }
362 
364 {
365  constraint_approximations_.clear();
366 }
367 
369 {
370  for (std::map<std::string, ConstraintApproximationPtr>::const_iterator it = constraint_approximations_.begin();
371  it != constraint_approximations_.end(); ++it)
372  {
373  out << it->second->getGroup() << std::endl;
374  out << it->second->getStateSpaceParameterization() << std::endl;
375  out << it->second->hasExplicitMotions() << std::endl;
376  out << it->second->getMilestoneCount() << std::endl;
377  out << it->second->getFilename() << std::endl;
378  out << it->second->getConstraintsMsg() << std::endl;
379  }
380 }
381 
382 const ompl_interface::ConstraintApproximationPtr&
383 ompl_interface::ConstraintsLibrary::getConstraintApproximation(const moveit_msgs::Constraints& msg) const
384 {
385  std::map<std::string, ConstraintApproximationPtr>::const_iterator it = constraint_approximations_.find(msg.name);
386  if (it != constraint_approximations_.end())
387  return it->second;
388 
389  static ConstraintApproximationPtr empty;
390  return empty;
391 }
392 
395  const moveit_msgs::Constraints& constr, const std::string& group,
396  const planning_scene::PlanningSceneConstPtr& scene, const ConstraintApproximationConstructionOptions& options)
397 {
398  return addConstraintApproximation(constr, constr, group, scene, options);
399 }
400 
403  const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard,
404  const std::string& group, const planning_scene::PlanningSceneConstPtr& scene,
406 {
408  ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, options.state_space_parameterization);
409  if (pc)
410  {
411  pc->clear();
412  pc->setPlanningScene(scene);
413  pc->setCompleteInitialState(scene->getCurrentState());
414 
416  ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, options, res);
417  ROS_INFO_NAMED("constraints_library", "Spent %lf seconds constructing the database",
418  (ros::WallTime::now() - start).toSec());
419  if (ss)
420  {
421  ConstraintApproximationPtr ca(new ConstraintApproximation(
422  group, options.state_space_parameterization, options.explicit_motions, constr_hard,
423  group + "_" + boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) +
424  ".ompldb",
425  ss, res.milestones));
426  if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end())
427  ROS_WARN_NAMED("constraints_library", "Overwriting constraint approximation named '%s'", ca->getName().c_str());
428  constraint_approximations_[ca->getName()] = ca;
429  res.approx = ca;
430  }
431  else
432  ROS_ERROR_NAMED("constraints_library", "Unable to construct constraint approximation for group '%s'",
433  group.c_str());
434  }
435  return res;
436 }
437 
439  const ModelBasedPlanningContextPtr& pcontext, const moveit_msgs::Constraints& constr_sampling,
440  const moveit_msgs::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options,
442 {
443  // state storage structure
444  ConstraintApproximationStateStorage* cass = new ConstraintApproximationStateStorage(pcontext->getOMPLStateSpace());
445  ob::StateStoragePtr sstor(cass);
446 
447  // construct a sampler for the sampling constraints
448  kinematic_constraints::KinematicConstraintSet kset(pcontext->getRobotModel());
449  robot_state::Transforms no_transforms(pcontext->getRobotModel()->getModelFrame());
450  kset.add(constr_hard, no_transforms);
451 
452  const robot_state::RobotState& default_state = pcontext->getCompleteInitialRobotState();
453 
454  unsigned int attempts = 0;
455 
456  double bounds_val = std::numeric_limits<double>::max() / 2.0 - 1.0;
457  pcontext->getOMPLStateSpace()->setPlanningVolume(-bounds_val, bounds_val, -bounds_val, bounds_val, -bounds_val,
458  bounds_val);
459  pcontext->getOMPLStateSpace()->setup();
460 
461  // construct the constrained states
462 
463  robot_state::RobotState kstate(default_state);
464  const constraint_samplers::ConstraintSamplerManagerPtr& csmng = pcontext->getConstraintSamplerManager();
465  ConstrainedSampler* csmp = NULL;
466  if (csmng)
467  {
468  constraint_samplers::ConstraintSamplerPtr cs =
469  csmng->selectSampler(pcontext->getPlanningScene(), pcontext->getJointModelGroup()->getName(), constr_sampling);
470  if (cs)
471  csmp = new ConstrainedSampler(pcontext.get(), cs);
472  }
473 
474  ob::StateSamplerPtr ss(csmp ? ob::StateSamplerPtr(csmp) : pcontext->getOMPLStateSpace()->allocDefaultStateSampler());
475 
476  ompl::base::ScopedState<> temp(pcontext->getOMPLStateSpace());
477  int done = -1;
478  bool slow_warn = false;
479  ompl::time::point start = ompl::time::now();
480  while (sstor->size() < options.samples)
481  {
482  ++attempts;
483  int done_now = 100 * sstor->size() / options.samples;
484  if (done != done_now)
485  {
486  done = done_now;
487  ROS_INFO_NAMED("constraints_library", "%d%% complete (kept %0.1lf%% sampled states)", done,
488  100.0 * (double)sstor->size() / (double)attempts);
489  }
490 
491  if (!slow_warn && attempts > 10 && attempts > sstor->size() * 100)
492  {
493  slow_warn = true;
494  ROS_WARN_NAMED("constraints_library", "Computation of valid state database is very slow...");
495  }
496 
497  if (attempts > options.samples && sstor->size() == 0)
498  {
499  ROS_ERROR_NAMED("constraints_library", "Unable to generate any samples");
500  break;
501  }
502 
503  ss->sampleUniform(temp.get());
504  pcontext->getOMPLStateSpace()->copyToRobotState(kstate, temp.get());
505  if (kset.decide(kstate).satisfied)
506  {
507  if (sstor->size() < options.samples)
508  {
509  temp->as<ModelBasedStateSpace::StateType>()->tag = sstor->size();
510  sstor->addState(temp.get());
511  }
512  }
513  }
514 
515  result.state_sampling_time = ompl::time::seconds(ompl::time::now() - start);
516  ROS_INFO_NAMED("constraints_library", "Generated %u states in %lf seconds", (unsigned int)sstor->size(),
517  result.state_sampling_time);
518  if (csmp)
519  {
521  ROS_INFO_NAMED("constraints_library", "Constrained sampling rate: %lf", result.sampling_success_rate);
522  }
523 
524  result.milestones = sstor->size();
525  if (options.edges_per_sample > 0)
526  {
527  ROS_INFO_NAMED("constraints_library", "Computing graph connections (max %u edges per sample) ...",
528  options.edges_per_sample);
529 
530  // construct connexions
531  const ob::StateSpacePtr& space = pcontext->getOMPLSimpleSetup()->getStateSpace();
532  unsigned int milestones = sstor->size();
533  std::vector<ob::State*> int_states(options.max_explicit_points, NULL);
534  pcontext->getOMPLSimpleSetup()->getSpaceInformation()->allocStates(int_states);
535 
536  ompl::time::point start = ompl::time::now();
537  int good = 0;
538  int done = -1;
539 
540  for (std::size_t j = 0; j < milestones; ++j)
541  {
542  int done_now = 100 * j / milestones;
543  if (done != done_now)
544  {
545  done = done_now;
546  ROS_INFO_NAMED("constraints_library", "%d%% complete", done);
547  }
548  if (cass->getMetadata(j).first.size() >= options.edges_per_sample)
549  continue;
550 
551  const ob::State* sj = sstor->getState(j);
552 
553  for (std::size_t i = j + 1; i < milestones; ++i)
554  {
555  if (cass->getMetadata(i).first.size() >= options.edges_per_sample)
556  continue;
557  double d = space->distance(sstor->getState(i), sj);
558  if (d >= options.max_edge_length)
559  continue;
560  unsigned int isteps =
561  std::min<unsigned int>(options.max_explicit_points, d / options.explicit_points_resolution);
562  double step = 1.0 / (double)isteps;
563  bool ok = true;
564  space->interpolate(sstor->getState(i), sj, step, int_states[0]);
565  for (unsigned int k = 1; k < isteps; ++k)
566  {
567  double this_step = step / (1.0 - (k - 1) * step);
568  space->interpolate(int_states[k - 1], sj, this_step, int_states[k]);
569  pcontext->getOMPLStateSpace()->copyToRobotState(kstate, int_states[k]);
570  if (!kset.decide(kstate).satisfied)
571  {
572  ok = false;
573  break;
574  }
575  }
576 
577  if (ok)
578  {
579  cass->getMetadata(i).first.push_back(j);
580  cass->getMetadata(j).first.push_back(i);
581 
582  if (options.explicit_motions)
583  {
584  cass->getMetadata(i).second[j].first = sstor->size();
585  for (unsigned int k = 0; k < isteps; ++k)
586  {
587  int_states[k]->as<ModelBasedStateSpace::StateType>()->tag = -1;
588  sstor->addState(int_states[k]);
589  }
590  cass->getMetadata(i).second[j].second = sstor->size();
591  cass->getMetadata(j).second[i] = cass->getMetadata(i).second[j];
592  }
593 
594  good++;
595  if (cass->getMetadata(j).first.size() >= options.edges_per_sample)
596  break;
597  }
598  }
599  }
600 
601  result.state_connection_time = ompl::time::seconds(ompl::time::now() - start);
602  ROS_INFO_NAMED("constraints_library", "Computed possible connexions in %lf seconds. Added %d connexions",
603  result.state_connection_time, good);
604  pcontext->getOMPLSimpleSetup()->getSpaceInformation()->freeStates(int_states);
605 
606  return sstor;
607  }
608 
609  // TODO(davetcoleman): this function did not originally return a value, causing compiler warnings in ROS Melodic
610  // Update with more intelligent logic as needed
611  ROS_ERROR_NAMED("constraints_library", "No StateStoragePtr found - implement better solution here.");
612  return sstor;
613 }
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 Mon Oct 15 2018 03:46:31