Loading...
Searching...
No Matches
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
41class ParallelBase
42{
43public:
44 virtual void getStart(Eigen::VectorXd &x) = 0;
45 virtual void getGoal(Eigen::VectorXd &x) = 0;
46};
47
48class ParallelChain : public ob::Constraint, public ParallelBase
49{
50public:
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
160private:
161 const Eigen::Vector3d offset_;
162 const unsigned int links_;
163 const unsigned int chainNum_;
164 const double length_;
165};
166
167class ParallelPlatform : public ob::Constraint, public ParallelBase
168{
169public:
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
221private:
222 const unsigned int links_;
223 const unsigned int chains_;
224 const double radius_;
225};
226
227class ParallelConstraint : public ob::ConstraintIntersection, public ParallelBase
228{
229public:
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
369private:
370 const unsigned int links_;
371 const unsigned int chains_;
372 const double radius_;
373 const double length_;
374 const double jointRadius_;
375};
376
377bool 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
398bool 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
410bool 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
435auto help_msg = "Shows this help message.";
436auto 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";
438auto links_msg = "Number of links in each kinematic chain. Minimum is 3. Must be odd.";
439auto chains_msg = "Number of chains in parallel mechanism. Minimum is 2.";
440auto bench_msg = "Do benchmarking on provided planner list.";
441
442int 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
std::vector< ConstraintPtr > constraints_
Constituent constraints.
Definition Constraint.h:293
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 constrainedPlanning...
Definition Constraint.h:76
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.
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...
STL namespace.
A class to store the exit status of Planner::solve()