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