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 }
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:107
Definition of an abstract state.
Definition: State.h:113
This namespace contains sampling based planning routines shared by both planning under geometric cons...
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
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:119
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
ConstraintIntersection(const unsigned int ambientDim, std::initializer_list< Constraint * > constraints)
Constructor. If constraints is empty assume it will be filled later.
Definition: Constraint.h:280
Definition of a constraint composed of multiple constraints that all must be satisfied simultaneously...
Definition: Constraint.h:275
void addExperimentParameter(const std::string &name, const std::string &type, const std::string &value)
Add an optional parameter's information to the benchmark output. Useful for aggregating results over ...
Definition: Benchmark.h:319
A class to store the exit status of Planner::solve()
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:112
std::vector< Constraint * > constraints_
Constituent constraints.
Definition: Constraint.h:322
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,...
Definition: Constraint.cpp:45
void setManifoldDimension(unsigned int k)
Sets the underlying manifold dimension.
Definition: Constraint.h:210
The exception type for ompl.
Definition: Exception.h:78
The lower and upper bounds for an Rn space.