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  logWarn("Manifest not found in folder '%s'. Not loading constraint approximations.", path.c_str());
274  return;
275  }
276 
277  logInform("Loading constrained space approximations from '%s'...", path.c_str());
278 
279  while (fin.good() && !fin.eof())
280  {
281  std::string group, state_space_parameterization, serialization, filename;
282  bool explicit_motions;
283  unsigned int milestones;
284  fin >> group;
285  if (fin.eof())
286  break;
287  fin >> state_space_parameterization;
288  if (fin.eof())
289  break;
290  fin >> explicit_motions;
291  if (fin.eof())
292  break;
293  fin >> milestones;
294  if (fin.eof())
295  break;
296  fin >> serialization;
297  if (fin.eof())
298  break;
299  fin >> filename;
300  logInform("Loading constraint approximation of type '%s' for group '%s' from '%s'...",
301  state_space_parameterization.c_str(), group.c_str(), filename.c_str());
302  const ModelBasedPlanningContextPtr& pc = context_manager_.getPlanningContext(group, state_space_parameterization);
303  if (pc)
304  {
305  moveit_msgs::Constraints msg;
306  hexToMsg(serialization, msg);
308  new ConstraintApproximationStateStorage(pc->getOMPLSimpleSetup()->getStateSpace());
309  cass->load((path + "/" + filename).c_str());
310  ConstraintApproximationPtr cap(new ConstraintApproximation(group, state_space_parameterization, explicit_motions,
311  msg, filename, ompl::base::StateStoragePtr(cass),
312  milestones));
313  if (constraint_approximations_.find(cap->getName()) != constraint_approximations_.end())
314  logWarn("Overwriting constraint approximation named '%s'", cap->getName().c_str());
315  constraint_approximations_[cap->getName()] = cap;
316  std::size_t sum = 0;
317  for (std::size_t i = 0; i < cass->size(); ++i)
318  sum += cass->getMetadata(i).first.size();
319  logInform("Loaded %lu states (%lu milestones) and %lu connections (%0.1lf per state) for constraint named '%s'%s",
320  cass->size(), cap->getMilestoneCount(), sum, (double)sum / (double)cap->getMilestoneCount(),
321  msg.name.c_str(), explicit_motions ? ". Explicit motions included." : "");
322  }
323  }
324  logInform("Done loading constrained space approximations.");
325 }
326 
328 {
329  logInform("Saving %u constrained space approximations to '%s'", (unsigned int)constraint_approximations_.size(),
330  path.c_str());
331  try
332  {
333  boost::filesystem::create_directories(path);
334  }
335  catch (...)
336  {
337  }
338 
339  std::ofstream fout((path + "/manifest").c_str());
340  if (fout.good())
341  for (std::map<std::string, ConstraintApproximationPtr>::const_iterator it = constraint_approximations_.begin();
342  it != constraint_approximations_.end(); ++it)
343  {
344  fout << it->second->getGroup() << std::endl;
345  fout << it->second->getStateSpaceParameterization() << std::endl;
346  fout << it->second->hasExplicitMotions() << std::endl;
347  fout << it->second->getMilestoneCount() << std::endl;
348  std::string serialization;
349  msgToHex(it->second->getConstraintsMsg(), serialization);
350  fout << serialization << std::endl;
351  fout << it->second->getFilename() << std::endl;
352  if (it->second->getStateStorage())
353  it->second->getStateStorage()->store((path + "/" + it->second->getFilename()).c_str());
354  }
355  else
356  logError("Unable to save constraint approximation to '%s'", path.c_str());
357  fout.close();
358 }
359 
361 {
362  constraint_approximations_.clear();
363 }
364 
366 {
367  for (std::map<std::string, ConstraintApproximationPtr>::const_iterator it = constraint_approximations_.begin();
368  it != constraint_approximations_.end(); ++it)
369  {
370  out << it->second->getGroup() << std::endl;
371  out << it->second->getStateSpaceParameterization() << std::endl;
372  out << it->second->hasExplicitMotions() << std::endl;
373  out << it->second->getMilestoneCount() << std::endl;
374  out << it->second->getFilename() << std::endl;
375  out << it->second->getConstraintsMsg() << std::endl;
376  }
377 }
378 
379 const ompl_interface::ConstraintApproximationPtr&
380 ompl_interface::ConstraintsLibrary::getConstraintApproximation(const moveit_msgs::Constraints& msg) const
381 {
382  std::map<std::string, ConstraintApproximationPtr>::const_iterator it = constraint_approximations_.find(msg.name);
383  if (it != constraint_approximations_.end())
384  return it->second;
385 
386  static ConstraintApproximationPtr empty;
387  return empty;
388 }
389 
392  const moveit_msgs::Constraints& constr, const std::string& group,
393  const planning_scene::PlanningSceneConstPtr& scene, const ConstraintApproximationConstructionOptions& options)
394 {
395  return addConstraintApproximation(constr, constr, group, scene, options);
396 }
397 
400  const moveit_msgs::Constraints& constr_sampling, const moveit_msgs::Constraints& constr_hard,
401  const std::string& group, const planning_scene::PlanningSceneConstPtr& scene,
403 {
405  ModelBasedPlanningContextPtr pc = context_manager_.getPlanningContext(group, options.state_space_parameterization);
406  if (pc)
407  {
408  pc->clear();
409  pc->setPlanningScene(scene);
410  pc->setCompleteInitialState(scene->getCurrentState());
411 
413  ompl::base::StateStoragePtr ss = constructConstraintApproximation(pc, constr_sampling, constr_hard, options, res);
414  logInform("Spent %lf seconds constructing the database", (ros::WallTime::now() - start).toSec());
415  if (ss)
416  {
417  ConstraintApproximationPtr ca(new ConstraintApproximation(
418  group, options.state_space_parameterization, options.explicit_motions, constr_hard,
419  group + "_" + boost::posix_time::to_iso_extended_string(boost::posix_time::microsec_clock::universal_time()) +
420  ".ompldb",
421  ss, res.milestones));
422  if (constraint_approximations_.find(ca->getName()) != constraint_approximations_.end())
423  logWarn("Overwriting constraint approximation named '%s'", ca->getName().c_str());
424  constraint_approximations_[ca->getName()] = ca;
425  res.approx = ca;
426  }
427  else
428  logError("Unable to construct constraint approximation for group '%s'", group.c_str());
429  }
430  return res;
431 }
432 
434  const ModelBasedPlanningContextPtr& pcontext, const moveit_msgs::Constraints& constr_sampling,
435  const moveit_msgs::Constraints& constr_hard, const ConstraintApproximationConstructionOptions& options,
437 {
438  // state storage structure
439  ConstraintApproximationStateStorage* cass = new ConstraintApproximationStateStorage(pcontext->getOMPLStateSpace());
440  ob::StateStoragePtr sstor(cass);
441 
442  // construct a sampler for the sampling constraints
443  kinematic_constraints::KinematicConstraintSet kset(pcontext->getRobotModel());
444  robot_state::Transforms no_transforms(pcontext->getRobotModel()->getModelFrame());
445  kset.add(constr_hard, no_transforms);
446 
447  const robot_state::RobotState& default_state = pcontext->getCompleteInitialRobotState();
448 
449  unsigned int attempts = 0;
450 
451  double bounds_val = std::numeric_limits<double>::max() / 2.0 - 1.0;
452  pcontext->getOMPLStateSpace()->setPlanningVolume(-bounds_val, bounds_val, -bounds_val, bounds_val, -bounds_val,
453  bounds_val);
454  pcontext->getOMPLStateSpace()->setup();
455 
456  // construct the constrained states
457 
458  robot_state::RobotState kstate(default_state);
459  const constraint_samplers::ConstraintSamplerManagerPtr& csmng = pcontext->getConstraintSamplerManager();
460  ConstrainedSampler* csmp = NULL;
461  if (csmng)
462  {
463  constraint_samplers::ConstraintSamplerPtr cs =
464  csmng->selectSampler(pcontext->getPlanningScene(), pcontext->getJointModelGroup()->getName(), constr_sampling);
465  if (cs)
466  csmp = new ConstrainedSampler(pcontext.get(), cs);
467  }
468 
469  ob::StateSamplerPtr ss(csmp ? ob::StateSamplerPtr(csmp) : pcontext->getOMPLStateSpace()->allocDefaultStateSampler());
470 
471  ompl::base::ScopedState<> temp(pcontext->getOMPLStateSpace());
472  int done = -1;
473  bool slow_warn = false;
474  ompl::time::point start = ompl::time::now();
475  while (sstor->size() < options.samples)
476  {
477  ++attempts;
478  int done_now = 100 * sstor->size() / options.samples;
479  if (done != done_now)
480  {
481  done = done_now;
482  logInform("%d%% complete (kept %0.1lf%% sampled states)", done, 100.0 * (double)sstor->size() / (double)attempts);
483  }
484 
485  if (!slow_warn && attempts > 10 && attempts > sstor->size() * 100)
486  {
487  slow_warn = true;
488  logWarn("Computation of valid state database is very slow...");
489  }
490 
491  if (attempts > options.samples && sstor->size() == 0)
492  {
493  logError("Unable to generate any samples");
494  break;
495  }
496 
497  ss->sampleUniform(temp.get());
498  pcontext->getOMPLStateSpace()->copyToRobotState(kstate, temp.get());
499  if (kset.decide(kstate).satisfied)
500  {
501  if (sstor->size() < options.samples)
502  {
503  temp->as<ModelBasedStateSpace::StateType>()->tag = sstor->size();
504  sstor->addState(temp.get());
505  }
506  }
507  }
508 
509  result.state_sampling_time = ompl::time::seconds(ompl::time::now() - start);
510  logInform("Generated %u states in %lf seconds", (unsigned int)sstor->size(), result.state_sampling_time);
511  if (csmp)
512  {
514  logInform("Constrained sampling rate: %lf", result.sampling_success_rate);
515  }
516 
517  result.milestones = sstor->size();
518  if (options.edges_per_sample > 0)
519  {
520  logInform("Computing graph connections (max %u edges per sample) ...", options.edges_per_sample);
521 
522  // construct connexions
523  const ob::StateSpacePtr& space = pcontext->getOMPLSimpleSetup()->getStateSpace();
524  unsigned int milestones = sstor->size();
525  std::vector<ob::State*> int_states(options.max_explicit_points, NULL);
526  pcontext->getOMPLSimpleSetup()->getSpaceInformation()->allocStates(int_states);
527 
528  ompl::time::point start = ompl::time::now();
529  int good = 0;
530  int done = -1;
531 
532  for (std::size_t j = 0; j < milestones; ++j)
533  {
534  int done_now = 100 * j / milestones;
535  if (done != done_now)
536  {
537  done = done_now;
538  logInform("%d%% complete", done);
539  }
540  if (cass->getMetadata(j).first.size() >= options.edges_per_sample)
541  continue;
542 
543  const ob::State* sj = sstor->getState(j);
544 
545  for (std::size_t i = j + 1; i < milestones; ++i)
546  {
547  if (cass->getMetadata(i).first.size() >= options.edges_per_sample)
548  continue;
549  double d = space->distance(sstor->getState(i), sj);
550  if (d >= options.max_edge_length)
551  continue;
552  unsigned int isteps =
553  std::min<unsigned int>(options.max_explicit_points, d / options.explicit_points_resolution);
554  double step = 1.0 / (double)isteps;
555  double remain = 1.0;
556  bool ok = true;
557  space->interpolate(sstor->getState(i), sj, step, int_states[0]);
558  for (unsigned int k = 1; k < isteps; ++k)
559  {
560  double this_step = step / (1.0 - (k - 1) * step);
561  space->interpolate(int_states[k - 1], sj, this_step, int_states[k]);
562  pcontext->getOMPLStateSpace()->copyToRobotState(kstate, int_states[k]);
563  if (!kset.decide(kstate).satisfied)
564  {
565  ok = false;
566  break;
567  }
568  }
569 
570  if (ok)
571  {
572  cass->getMetadata(i).first.push_back(j);
573  cass->getMetadata(j).first.push_back(i);
574 
575  if (options.explicit_motions)
576  {
577  cass->getMetadata(i).second[j].first = sstor->size();
578  for (unsigned int k = 0; k < isteps; ++k)
579  {
580  int_states[k]->as<ModelBasedStateSpace::StateType>()->tag = -1;
581  sstor->addState(int_states[k]);
582  }
583  cass->getMetadata(i).second[j].second = sstor->size();
584  cass->getMetadata(j).second[i] = cass->getMetadata(i).second[j];
585  }
586 
587  good++;
588  if (cass->getMetadata(j).first.size() >= options.edges_per_sample)
589  break;
590  }
591  }
592  }
593 
594  result.state_connection_time = ompl::time::seconds(ompl::time::now() - start);
595  logInform("Computed possible connexions in %lf seconds. Added %d connexions", result.state_connection_time, good);
596  pcontext->getOMPLSimpleSetup()->getSpaceInformation()->freeStates(int_states);
597 
598  return sstor;
599  }
600 }
d
std::pair< std::vector< std::size_t >, std::map< std::size_t, std::pair< std::size_t, std::size_t > > > ConstrainedStateMetadata
filename
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
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 Apr 18 2018 02:49:46