ConstrainedPlanningImplicitParallel.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Rice University
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 the Rice University 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: Zachary Kingston */
36 
37 #include "ConstrainedPlanningCommon.h"
38 
39 class ParallelBase
40 {
41 public:
42  virtual void getStart(Eigen::VectorXd &x) = 0;
43  virtual void getGoal(Eigen::VectorXd &x) = 0;
44 };
45 
46 class ParallelChain : public ob::Constraint, public ParallelBase
47 {
48 public:
49  ParallelChain(const unsigned int n, Eigen::Vector3d offset, unsigned int links, unsigned int chainNum,
50  double length = 1, double jointRadius = 0.2)
51  : ob::Constraint(n, links)
52  , offset_(offset)
53  , links_(links)
54  , chainNum_(chainNum)
55  , length_(length)
56  , jointRadius_(jointRadius)
57  {
58  if (links % 2 == 0)
59  throw ompl::Exception("Number of links must be odd!");
60  }
61 
62  void getStart(Eigen::VectorXd &x) override
63  {
64  const double angle = boost::math::constants::pi<double>() / 16;
65  const unsigned int offset = 3 * links_ * chainNum_;
66  const Eigen::VectorXd axis =
67  Eigen::AngleAxisd(boost::math::constants::pi<double>() / 2, Eigen::Vector3d::UnitZ()) * offset_;
68 
69  const Eigen::VectorXd step = Eigen::Vector3d::UnitZ() * length_;
70  Eigen::VectorXd joint = offset_ + Eigen::AngleAxisd(angle, axis) * step;
71 
72  unsigned int i = 0;
73  for (; i < links_; ++i)
74  {
75  x.segment(3 * i + offset, 3) = joint;
76  if (i < links_ - 2)
77  joint += step;
78  else
79  joint += Eigen::AngleAxisd(-angle, axis) * step;
80  }
81  }
82 
83  void getGoal(Eigen::VectorXd &x) override
84  {
85  unsigned int offset = 3 * links_ * chainNum_;
86 
87  if (links_ == 7)
88  {
89  Eigen::VectorXd nstep = offset_ * length_;
90  Eigen::VectorXd estep =
91  Eigen::AngleAxisd(boost::math::constants::pi<double>() / 2, Eigen::Vector3d::UnitZ()) * offset_ *
92  length_;
93  Eigen::VectorXd sstep =
94  Eigen::AngleAxisd(boost::math::constants::pi<double>(), Eigen::Vector3d::UnitZ()) * offset_ * length_;
95  Eigen::VectorXd wstep =
96  Eigen::AngleAxisd(3 * boost::math::constants::pi<double>() / 2, Eigen::Vector3d::UnitZ()) * offset_ *
97  length_;
98 
99  Eigen::VectorXd joint = offset_ + nstep;
100  x.segment(3 * 0 + offset, 3) = joint;
101  x.segment(3 * 1 + offset, 3) = x.segment(3 * 0 + offset, 3) + estep;
102  x.segment(3 * 2 + offset, 3) = x.segment(3 * 1 + offset, 3) + estep;
103  x.segment(3 * 3 + offset, 3) = x.segment(3 * 2 + offset, 3) + Eigen::Vector3d::UnitZ() * length_;
104  x.segment(3 * 4 + offset, 3) = x.segment(3 * 3 + offset, 3) + sstep;
105  x.segment(3 * 5 + offset, 3) = x.segment(3 * 4 + offset, 3) + sstep;
106  x.segment(3 * 6 + offset, 3) = x.segment(3 * 5 + offset, 3) + wstep;
107  }
108  else
109  {
110  Eigen::VectorXd step = offset_ * length_;
111  Eigen::VectorXd joint = offset_ + step;
112 
113  unsigned int i = 0;
114  for (; i < links_ / 2; ++i, joint += step)
115  x.segment(3 * i + offset, 3) = joint;
116 
117  joint += Eigen::Vector3d::UnitZ() * length_ - step;
118  for (; i < links_; ++i, joint -= step)
119  x.segment(3 * i + offset, 3) = joint;
120  }
121  }
122 
123  Eigen::Ref<const Eigen::VectorXd> getLink(const Eigen::VectorXd &x, const unsigned int idx) const
124  {
125  const unsigned int offset = 3 * links_ * chainNum_;
126  return x.segment(offset + 3 * idx, 3);
127  }
128 
129  void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
130  {
131  unsigned int idx = 0;
132 
133  Eigen::VectorXd j1 = offset_;
134  for (unsigned int i = 0; i < links_; ++i)
135  {
136  const Eigen::VectorXd j2 = getLink(x, i);
137  out[idx++] = (j1 - j2).norm() - length_;
138  j1 = j2;
139  }
140  }
141 
142  void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
143  {
144  const unsigned int offset = 3 * links_ * chainNum_;
145  out.setZero();
146 
147  Eigen::VectorXd plus(3 * (links_ + 1));
148  plus.head(3 * links_) = x.segment(offset, 3 * links_);
149  plus.tail(3) = Eigen::VectorXd::Zero(3);
150 
151  Eigen::VectorXd minus(3 * (links_ + 1));
152  minus.head(3) = offset_;
153  minus.tail(3 * links_) = x.segment(offset, 3 * links_);
154 
155  const Eigen::VectorXd diagonal = plus - minus;
156 
157  for (unsigned int i = 0; i < links_; i++)
158  out.row(i).segment(3 * i + offset, 3) = diagonal.segment(3 * i, 3).normalized();
159 
160  out.block(1, offset, links_ - 1, 3 * links_ - 3) -= out.block(1, offset + 3, links_ - 1, 3 * links_ - 3);
161  }
162 
163 private:
164  const Eigen::Vector3d offset_;
165  const unsigned int links_;
166  const unsigned int chainNum_;
167  const double length_;
168  const double jointRadius_;
169 };
170 
171 class ParallelPlatform : public ob::Constraint, public ParallelBase
172 {
173 public:
174  ParallelPlatform(unsigned int links, unsigned int chains, double radius = 1)
175  : ob::Constraint(3 * links * chains, chains), links_(links), chains_(chains), radius_(radius)
176  {
177  if (chains == 2)
178  setManifoldDimension(k_ + 1);
179 
180  if (chains >= 4)
181  setManifoldDimension(k_ - (chains - 3));
182  }
183 
184  Eigen::Ref<const Eigen::VectorXd> getTip(const Eigen::VectorXd &x, unsigned int id) const
185  {
186  return x.segment(3 * links_ * ((id % chains_) + 1) - 3, 3);
187  }
188 
189  void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
190  {
191  if (chains_ == 2)
192  {
193  out[0] = (getTip(x, 0) - getTip(x, 1)).norm() - radius_ * 2;
194  return;
195  }
196 
197  unsigned int idx = 0;
198 
199  Eigen::Vector3d centroid = Eigen::Vector3d::Zero();
200  for (unsigned int i = 0; i < chains_; ++i)
201  centroid += getTip(x, i);
202  centroid /= chains_;
203 
204  for (unsigned int i = 0; i < chains_; ++i)
205  out[idx++] = (centroid - getTip(x, i)).norm() - radius_;
206 
207  for (unsigned int i = 0; i < chains_ - 3; ++i)
208  {
209  const Eigen::Vector3d ab = getTip(x, i + 1) - getTip(x, i);
210  const Eigen::Vector3d ac = getTip(x, i + 2) - getTip(x, i);
211  const Eigen::Vector3d ad = getTip(x, i + 3) - getTip(x, i);
212 
213  out[idx++] = ad.dot(ab.cross(ac));
214  }
215  }
216 
217  void getStart(Eigen::VectorXd &x) override
218  {
219  }
220 
221  void getGoal(Eigen::VectorXd &x) override
222  {
223  }
224 
225 private:
226  const unsigned int links_;
227  const unsigned int chains_;
228  const double radius_;
229 };
230 
231 class ParallelConstraint : public ob::ConstraintIntersection, public ParallelBase
232 {
233 public:
234  ParallelConstraint(unsigned int links, unsigned int chains, double radius = 1, double length = 1,
235  double jointRadius = 0.2)
236  : ob::ConstraintIntersection(3 * links * chains, {})
237  , links_(links)
238  , chains_(chains)
239  , radius_(radius)
240  , length_(length)
241  , jointRadius_(jointRadius)
242  {
243  Eigen::Vector3d offset = Eigen::Vector3d::UnitX();
244  for (unsigned int i = 0; i < chains_; ++i)
245  {
246  addConstraint(new ParallelChain(chains * links * 3, offset, links, i, length, jointRadius));
247  offset =
248  Eigen::AngleAxisd(2 * boost::math::constants::pi<double>() / (double)chains, Eigen::Vector3d::UnitZ()) *
249  offset;
250  }
251 
252  addConstraint(new ParallelPlatform(links, chains, radius));
253  }
254 
255  void getStart(Eigen::VectorXd &x) override
256  {
257  x = Eigen::VectorXd(3 * links_ * chains_);
258  for (unsigned int i = 0; i < constraints_.size(); ++i)
259  dynamic_cast<ParallelBase *>(constraints_[i])->getStart(x);
260  }
261 
262  void getGoal(Eigen::VectorXd &x) override
263  {
264  x = Eigen::VectorXd(3 * links_ * chains_);
265  for (unsigned int i = 0; i < constraints_.size(); ++i)
266  dynamic_cast<ParallelBase *>(constraints_[i])->getGoal(x);
267  }
268 
269  ob::StateSpacePtr createSpace() const
270  {
271  auto rvss = std::make_shared<ob::RealVectorStateSpace>(3 * links_ * chains_);
272  ob::RealVectorBounds bounds(3 * links_ * chains_);
273 
274  for (unsigned int c = 0; c < chains_; ++c)
275  {
276  const unsigned int o = 3 * c * links_;
277  for (int i = 0; i < (int)links_; ++i)
278  {
279  bounds.setLow(o + 3 * i + 0, -i - 2);
280  bounds.setHigh(o + 3 * i + 0, i + 2);
281 
282  bounds.setLow(o + 3 * i + 1, -i - 2);
283  bounds.setHigh(o + 3 * i + 1, i + 2);
284 
285  bounds.setLow(o + 3 * i + 2, -i - 2);
286  bounds.setHigh(o + 3 * i + 2, i + 2);
287  }
288  }
289 
290  rvss->setBounds(bounds);
291  return rvss;
292  }
293 
294  bool isValid(const ob::State *state)
295  {
296  auto &&x = *state->as<ob::ConstrainedStateSpace::StateType>();
297 
298  for (unsigned int i = 0; i < links_ * chains_; ++i)
299  {
300  if (x.segment(3 * i, 3)[2] < 0)
301  return false;
302  }
303 
304  for (unsigned int i = 0; i < links_ * chains_ - 1; ++i)
305  {
306  if (x.segment(3 * i, 3).cwiseAbs().maxCoeff() < jointRadius_)
307  return false;
308 
309  for (unsigned int j = i + 1; j < links_ * chains_; ++j)
310  if ((x.segment(3 * i, 3) - x.segment(3 * j, 3)).cwiseAbs().maxCoeff() < jointRadius_)
311  return false;
312  }
313 
314  return true;
315  }
316 
319  ob::ProjectionEvaluatorPtr getProjection(ob::StateSpacePtr space) const
320  {
321  class ParallelProjection : public ob::ProjectionEvaluator
322  {
323  public:
324  ParallelProjection(ob::StateSpacePtr space, unsigned int links, unsigned int chains)
325  : ob::ProjectionEvaluator(space), chains_(chains), links_(links)
326  {
327  }
328 
329  unsigned int getDimension(void) const
330  {
331  return 1;
332  }
333 
334  void defaultCellSizes(void)
335  {
336  cellSizes_.resize(1);
337  cellSizes_[0] = 0.1;
338  }
339 
340  void project(const ob::State *state, Eigen::Ref<Eigen::VectorXd> projection) const
341  {
342  auto &&x = *state->as<ob::ConstrainedStateSpace::StateType>();
343 
344  for (unsigned int i = 0; i < chains_; ++i)
345  projection(0) = x[3 * (i + 1) * links_ - 1];
346 
347  projection(0) /= chains_;
348  }
349 
350  private:
351  const unsigned int chains_;
352  const unsigned int links_;
353  };
354 
355  return std::make_shared<ParallelProjection>(space, links_, chains_);
356  }
357 
358  void dump(std::ofstream &file) const
359  {
360  file << links_ << std::endl;
361  file << chains_ << std::endl;
362  file << jointRadius_ << std::endl;
363  file << length_ << std::endl;
364  file << radius_ << std::endl;
365  }
366 
367  void addBenchmarkParameters(ot::Benchmark *bench) const
368  {
369  bench->addExperimentParameter("links", "INTEGER", std::to_string(links_));
370  bench->addExperimentParameter("chains", "INTEGER", std::to_string(chains_));
371  }
372 
373 private:
374  const unsigned int links_;
375  const unsigned int chains_;
376  const double radius_;
377  const double length_;
378  const double jointRadius_;
379 };
380 
381 bool parallelPlanningOnce(ConstrainedProblem &cp, enum PLANNER_TYPE planner, bool output)
382 {
383  cp.setPlanner(planner, "parallel");
384 
385  // Solve the problem
386  ob::PlannerStatus stat = cp.solveOnce(output, "parallel");
387 
388  if (output)
389  {
390  OMPL_INFORM("Dumping problem information to `parallel_info.txt`.");
391  std::ofstream infofile("parallel_info.txt");
392  infofile << cp.type << std::endl;
393  dynamic_cast<ParallelConstraint *>(cp.constraint.get())->dump(infofile);
394  infofile.close();
395  }
396 
397  cp.atlasStats();
398 
399  return stat;
400 }
401 
402 bool parallelPlanningBench(ConstrainedProblem &cp, std::vector<enum PLANNER_TYPE> &planners)
403 {
404  cp.setupBenchmark(planners, "parallel");
405 
406  auto parallel = dynamic_cast<ParallelConstraint *>(cp.constraint.get());
407  parallel->addBenchmarkParameters(cp.bench);
408 
409  cp.runBenchmark();
410 
411  return 0;
412 }
413 
414 bool parallelPlanning(bool output, enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners, unsigned int links,
415  unsigned int chains, struct ConstrainedOptions &c_opt, struct AtlasOptions &a_opt, bool bench)
416 {
417  // Create a shared pointer to our constraint.
418  auto constraint = std::make_shared<ParallelConstraint>(links, chains);
419 
420  ConstrainedProblem cp(space, constraint->createSpace(), constraint);
421  cp.setConstrainedOptions(c_opt);
422  cp.setAtlasOptions(a_opt);
423 
424  cp.css->registerProjection("parallel", constraint->getProjection(cp.css));
425 
426  Eigen::VectorXd start, goal;
427  constraint->getStart(start);
428  constraint->getGoal(goal);
429 
430  cp.setStartAndGoalStates(start, goal);
431  cp.ss->setStateValidityChecker(std::bind(&ParallelConstraint::isValid, constraint, std::placeholders::_1));
432 
433  if (!bench)
434  return parallelPlanningOnce(cp, planners[0], output);
435  else
436  return parallelPlanningBench(cp, planners);
437 }
438 
439 auto help_msg = "Shows this help message.";
440 auto output_msg = "Dump found solution path (if one exists) in plain text to `parallel_path.txt`. "
441  "Problem information is dumped to `parallel_info`.txt";
442 auto links_msg = "Number of links in each kinematic chain. Minimum is 3. Must be odd.";
443 auto chains_msg = "Number of chains in parallel mechanism. Minimum is 2.";
444 auto bench_msg = "Do benchmarking on provided planner list.";
445 
446 int main(int argc, char **argv)
447 {
448  bool output, bench;
449  enum SPACE_TYPE space = PJ;
450  std::vector<enum PLANNER_TYPE> planners = {RRT};
451 
452  unsigned int links = 3;
453  unsigned int chains = 4;
454 
455  struct ConstrainedOptions c_opt;
456  struct AtlasOptions a_opt;
457 
458  po::options_description desc("Options");
459  desc.add_options()("help,h", help_msg);
460  desc.add_options()("output,o", po::bool_switch(&output)->default_value(false), output_msg);
461  desc.add_options()("links,l", po::value<unsigned int>(&links)->default_value(3), links_msg);
462  desc.add_options()("chains,c", po::value<unsigned int>(&chains)->default_value(4), chains_msg);
463  desc.add_options()("bench", po::bool_switch(&bench)->default_value(false), bench_msg);
464 
465  addSpaceOption(desc, &space);
466  addPlannerOption(desc, &planners);
467  addConstrainedOptions(desc, &c_opt);
468  addAtlasOptions(desc, &a_opt);
469 
470  po::variables_map vm;
471  po::store(po::parse_command_line(argc, argv, desc), vm);
472  po::notify(vm);
473 
474  if (vm.count("help"))
475  {
476  std::cout << desc << std::endl;
477  return 1;
478  }
479 
480  parallelPlanning(output, space, planners, links, chains, c_opt, a_opt, bench);
481 
482  return 0;
483 }
A shared pointer wrapper for ompl::base::StateSpace.
ConstraintIntersection(const unsigned int ambientDim, std::initializer_list< Constraint *> constraints)
Constructor. If constraints is empty assume it will be filled later.
Definition: Constraint.h:248
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:48
Constraint(const unsigned int ambientDim, const unsigned int coDim, double tolerance=magic::CONSTRAINT_PROJECTION_TOLERANCE)
Constructor. The dimension of the ambient configuration space as well as the dimension of the functio...
Definition: Constraint.h:87
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
void addExperimentParameter(const std::string &name, const std::string &type, const std::string &value)
Add an optional parameter&#39;s information to the benchmark output. Useful for aggregating results over ...
Definition: Benchmark.h:223
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:75
void jacobian(const State *state, Eigen::Ref< Eigen::MatrixXd > out) const
Compute the Jacobian of the constraint function at state. Result is returned in out, which should be allocated to size coDim by ambientDim. Default implementation performs the differentiation numerically with a seven-point central difference stencil. It is best to provide an analytic formulation.
Definition: Constraint.cpp:45
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
The exception type for ompl.
Definition: Exception.h:46
The lower and upper bounds for an Rn space.
void setManifoldDimension(unsigned int k)
Sets the underlying manifold dimension.
Definition: Constraint.h:178
std::vector< Constraint * > constraints_
Constituent constraints.
Definition: Constraint.h:290
Definition of a constraint composed of multiple constraints that all must be satisfied simultaneously...
Definition: Constraint.h:243
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68