Loading...
Searching...
No Matches
ConstrainedPlanningImplicitChain.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
39class ChainConstraint : public ob::Constraint
40{
41private:
45 class Wall
46 {
47 public:
48 Wall(double offset, double thickness, double width, double joint_radius, unsigned int type)
49 : offset_(offset), thickness_(thickness + joint_radius), width_(width + joint_radius), type_(type)
50 {
51 }
52
56 bool within(double x) const
57 {
58 return !(x < (offset_ - thickness_) || x > (offset_ + thickness_));
59 }
60
61 bool checkJoint(const Eigen::Ref<const Eigen::VectorXd> &v) const
62 {
63 double x = v[0], y = v[1], z = v[2];
64
65 if (!within(x))
66 return true;
67
68 if (z <= width_)
69 {
70 switch (type_)
71 {
72 case 0:
73 if (y < 0)
74 return true;
75 break;
76
77 case 1:
78 if (y > 0)
79 return true;
80 break;
81 }
82 }
83
84 return false;
85 }
86
87 private:
88 const double offset_;
89 const double thickness_;
90 const double width_;
91 const unsigned int type_;
92 };
93
94 const double WALL_WIDTH = 0.5;
95 const double JOINT_RADIUS = 0.2;
96 const double LINK_LENGTH = 1.0;
97
98public:
111 ChainConstraint(unsigned int links, unsigned int obstacles = 0, unsigned int extra = 1)
112 : ob::Constraint(3 * links, links + extra)
113 , links_(links)
114 , length_(LINK_LENGTH)
115 , width_(WALL_WIDTH)
116 , radius_(links - 2)
117 , jointRadius_(JOINT_RADIUS)
118 , obstacles_(obstacles)
119 , extra_(extra)
120 {
121 double step = 2 * radius_ / (double)(obstacles_ + 1);
122 double current = -radius_ + step;
123
124 for (unsigned int i = 0; i < obstacles_; i++, current += step)
125 walls_.emplace_back(current, radius_ / 8, WALL_WIDTH, JOINT_RADIUS, i % 2);
126 }
127
128 void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
129 {
130 Eigen::VectorXd joint1 = Eigen::VectorXd::Zero(3);
131 for (unsigned int i = 0; i < links_; i++)
132 {
133 auto &&joint2 = x.segment(3 * i, 3);
134 out[i] = (joint1 - joint2).norm() - length_;
135 joint1 = joint2;
136 }
137
138 if (extra_ >= 1)
139 out[links_] = x.tail(3).norm() - radius_;
140
141 const unsigned int o = links_ - 5;
142
143 if (extra_ >= 2)
144 out[links_ + 1] = x[(o + 0) * 3 + 2] - x[(o + 1) * 3 + 2];
145 if (extra_ >= 3)
146 out[links_ + 2] = x[(o + 1) * 3 + 0] - x[(o + 2) * 3 + 0];
147 if (extra_ >= 4)
148 out[links_ + 3] = x[(o + 2) * 3 + 2] - x[(o + 3) * 3 + 2];
149 }
150
151 void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
152 {
153 out.setZero();
154
155 Eigen::VectorXd plus(3 * (links_ + 1));
156 plus.head(3 * links_) = x.segment(0, 3 * links_);
157 plus.tail(3) = Eigen::VectorXd::Zero(3);
158
159 Eigen::VectorXd minus(3 * (links_ + 1));
160 minus.head(3) = Eigen::VectorXd::Zero(3);
161 minus.tail(3 * links_) = x.segment(0, 3 * links_);
162
163 auto &&diagonal = plus - minus;
164
165 for (unsigned int i = 0; i < links_; i++)
166 out.row(i).segment(3 * i + 0, 3) = diagonal.segment(3 * i, 3).normalized();
167
168 out.block(1, 0, links_ - 1, 3 * links_ - 3) -= out.block(1, 3, links_ - 1, 3 * links_ - 3);
169
170 if (extra_ >= 1)
171 out.row(links_).tail(3) = -diagonal.tail(3).normalized().transpose();
172
173 const unsigned int o = links_ - 5;
174
175 if (extra_ >= 2)
176 {
177 out(links_ + 1, (o + 0) * 3 + 2) = 1;
178 out(links_ + 1, (o + 1) * 3 + 2) = -1;
179 }
180 if (extra_ >= 3)
181 {
182 out(links_ + 2, (o + 1) * 3 + 0) = 1;
183 out(links_ + 2, (o + 2) * 3 + 0) = -1;
184 }
185 if (extra_ >= 4)
186 {
187 out(links_ + 3, (o + 2) * 3 + 2) = 1;
188 out(links_ + 3, (o + 3) * 3 + 2) = -1;
189 }
190 }
191
195 bool isValid(const ob::State *state)
196 {
197 auto &&x = *state->as<ob::ConstrainedStateSpace::StateType>();
198
199 for (unsigned int i = 0; i < links_; i++)
200 {
201 auto &&joint = x.segment(3 * i, 3);
202 if (joint[2] < 0)
203 return false;
204
205 if (joint.norm() >= (radius_ - jointRadius_))
206 for (auto wall : walls_)
207 if (!wall.checkJoint(joint))
208 return false;
209 }
210
211 for (unsigned int i = 0; i < links_ - 1; i++)
212 {
213 auto &&joint1 = x.segment(3 * i, 3);
214 if (joint1.cwiseAbs().maxCoeff() < jointRadius_)
215 return false;
216
217 for (unsigned int j = i + 1; j < links_; j++)
218 {
219 auto &&joint2 = x.segment(3 * j, 3);
220 if ((joint1 - joint2).cwiseAbs().maxCoeff() < jointRadius_)
221 return false;
222 }
223 }
224
225 return true;
226 }
227
228 ob::StateSpacePtr createSpace() const
229 {
230 auto rvss = std::make_shared<ob::RealVectorStateSpace>(3 * links_);
231 ob::RealVectorBounds bounds(3 * links_);
232
233 for (int i = 0; i < (int)links_; ++i)
234 {
235 bounds.setLow(3 * i + 0, -i - 1);
236 bounds.setHigh(3 * i + 0, i + 1);
237
238 bounds.setLow(3 * i + 1, -i - 1);
239 bounds.setHigh(3 * i + 1, i + 1);
240
241 bounds.setLow(3 * i + 2, -i - 1);
242 bounds.setHigh(3 * i + 2, i + 1);
243 }
244
245 rvss->setBounds(bounds);
246 return rvss;
247 }
248
249 void setStartAndGoalStates(Eigen::VectorXd &start, Eigen::VectorXd &goal) const
250 {
251 start = Eigen::VectorXd(3 * links_);
252 goal = Eigen::VectorXd(3 * links_);
253
254 int i = 0;
255 for (; i < (int)links_ - 3; ++i)
256 {
257 start[3 * i] = i + 1;
258 start[3 * i + 1] = 0;
259 start[3 * i + 2] = 0;
260
261 goal[3 * i] = -(i + 1);
262 goal[3 * i + 1] = 0;
263 goal[3 * i + 2] = 0;
264 }
265
266 start[3 * i] = i;
267 start[3 * i + 1] = -1;
268 start[3 * i + 2] = 0;
269
270 goal[3 * i] = -i;
271 goal[3 * i + 1] = 1;
272 goal[3 * i + 2] = 0;
273
274 i++;
275
276 start[3 * i] = i;
277 start[3 * i + 1] = -1;
278 start[3 * i + 2] = 0;
279
280 goal[3 * i] = -i;
281 goal[3 * i + 1] = 1;
282 goal[3 * i + 2] = 0;
283
284 i++;
285
286 start[3 * i] = i - 1;
287 start[3 * i + 1] = 0;
288 start[3 * i + 2] = 0;
289
290 goal[3 * i] = -(i - 1);
291 goal[3 * i + 1] = 0;
292 goal[3 * i + 2] = 0;
293 }
294
298 ob::ProjectionEvaluatorPtr getProjection(ob::StateSpacePtr space) const
299 {
300 class ChainProjection : public ob::ProjectionEvaluator
301 {
302 public:
303 ChainProjection(const ob::StateSpacePtr &space, unsigned int links, double radius)
304 : ob::ProjectionEvaluator(space), links_(links), radius_(radius)
305 {
306 }
307
308 unsigned int getDimension() const override
309 {
310 return 2;
311 }
312
313 void defaultCellSizes() override
314 {
315 cellSizes_.resize(2);
316 cellSizes_[0] = 0.1;
317 cellSizes_[1] = 0.1;
318 }
319
320 void project(const ob::State *state, Eigen::Ref<Eigen::VectorXd> projection) const override
321 {
322 auto &&x = *state->as<ob::ConstrainedStateSpace::StateType>();
323 const unsigned int s = 3 * (links_ - 1);
324
325 projection(0) = atan2(x[s + 1], x[s]);
326 projection(1) = acos(x[s + 2] / radius_);
327 }
328
329 private:
330 const unsigned int links_; // Number of chain links.
331 double radius_; // Radius of sphere end-effector lies on (for extra = 1)
332 };
333
334 return std::make_shared<ChainProjection>(space, links_, radius_);
335 }
336
337 void dump(std::ofstream &file) const
338 {
339 file << links_ << std::endl;
340 file << obstacles_ << std::endl;
341 file << extra_ << std::endl;
342 file << jointRadius_ << std::endl;
343 file << length_ << std::endl;
344 file << radius_ << std::endl;
345 file << width_ << std::endl;
346 }
347
348 void addBenchmarkParameters(ot::Benchmark *bench) const
349 {
350 bench->addExperimentParameter("links", "INTEGER", std::to_string(links_));
351 bench->addExperimentParameter("obstacles", "INTEGER", std::to_string(obstacles_));
352 bench->addExperimentParameter("extra", "INTEGER", std::to_string(extra_));
353 }
354
355private:
356 const unsigned int links_; // Number of chain links.
357 const double length_; // Length of one link.
358 const double width_; // Width of obstacle wall.
359 const double radius_; // Radius of the sphere that the end effector is constrained to.
360 const double jointRadius_; // Size of joints
361 const unsigned int obstacles_; // Number of obstacles on sphere surface
362 const unsigned int extra_; // Number of extra constraints
363 std::vector<Wall> walls_; // Obstacles
364};
365
366bool chainPlanningOnce(ConstrainedProblem &cp, enum PLANNER_TYPE planner, bool output)
367{
368 cp.setPlanner(planner, "chain");
369
370 // Solve the problem
371 ob::PlannerStatus stat = cp.solveOnce(output, "chain");
372
373 if (output)
374 {
375 OMPL_INFORM("Dumping problem information to `chain_info.txt`.");
376 std::ofstream infofile("chain_info.txt");
377 infofile << cp.type << std::endl;
378 dynamic_cast<ChainConstraint *>(cp.constraint.get())->dump(infofile);
379 infofile.close();
380 }
381
382 cp.atlasStats();
383
384 return stat;
385}
386
387bool chainPlanningBench(ConstrainedProblem &cp, std::vector<enum PLANNER_TYPE> &planners)
388{
389 cp.setupBenchmark(planners, "chain");
390
391 auto chain = dynamic_cast<ChainConstraint *>(cp.constraint.get());
392 chain->addBenchmarkParameters(cp.bench);
393
394 cp.runBenchmark();
395
396 return false;
397}
398
399bool chainPlanning(bool output, enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners, unsigned int links,
400 unsigned int obstacles, unsigned int extra, struct ConstrainedOptions &c_opt,
401 struct AtlasOptions &a_opt, bool bench)
402{
403 // Create a shared pointer to our constraint.
404 auto constraint = std::make_shared<ChainConstraint>(links, obstacles, extra);
405
406 ConstrainedProblem cp(space, constraint->createSpace(), constraint);
407 cp.setConstrainedOptions(c_opt);
408 cp.setAtlasOptions(a_opt);
409
410 cp.css->registerProjection("chain", constraint->getProjection(cp.css));
411
412 Eigen::VectorXd start, goal;
413 constraint->setStartAndGoalStates(start, goal);
414
415 cp.setStartAndGoalStates(start, goal);
416 cp.ss->setStateValidityChecker(std::bind(&ChainConstraint::isValid, constraint, std::placeholders::_1));
417
418 if (!bench)
419 return chainPlanningOnce(cp, planners[0], output);
420 else
421 return chainPlanningBench(cp, planners);
422}
423
424auto help_msg = "Shows this help message.";
425auto output_msg = "Dump found solution path (if one exists) in plain text to `chain_path.txt`. "
426 "Problem information is dumped to `chain_info`.txt";
427auto links_msg = "Number of links in the kinematic chain. Minimum is 4.";
428auto obstacles_msg = "Number of `wall' obstacles on the surface of the sphere. Ranges from [0, 2]";
429auto extra_msg = "Number of extra constraints to add to the chain. Extra constraints are as follows:\n"
430 "1: End-effector is constrained to be on the surface of a sphere of radius links - 2\n"
431 "2: (links-5)th and (links-4)th ball have the same z-value\n"
432 "3: (links-4)th and (links-3)th ball have the same x-value\n"
433 "4: (links-3)th and (links-2)th ball have the same z-value";
434auto bench_msg = "Do benchmarking on provided planner list.";
435
436int main(int argc, char **argv)
437{
438 bool output, bench;
439 enum SPACE_TYPE space = PJ;
440 std::vector<enum PLANNER_TYPE> planners = {RRT};
441
442 unsigned int links = 5;
443 unsigned int obstacles = 0;
444 unsigned int extra = 1;
445
446 struct ConstrainedOptions c_opt;
447 struct AtlasOptions a_opt;
448
449 po::options_description desc("Options");
450 desc.add_options()("help,h", help_msg);
451 desc.add_options()("output,o", po::bool_switch(&output)->default_value(false), output_msg);
452 desc.add_options()("links,l", po::value<unsigned int>(&links)->default_value(5), links_msg);
453 desc.add_options()("obstacles,x", po::value<unsigned int>(&obstacles)->default_value(0), obstacles_msg);
454 desc.add_options()("extra,e", po::value<unsigned int>(&extra)->default_value(1), extra_msg);
455 desc.add_options()("bench", po::bool_switch(&bench)->default_value(false), bench_msg);
456
457 addSpaceOption(desc, &space);
458 addPlannerOption(desc, &planners);
459 addConstrainedOptions(desc, &c_opt);
460 addAtlasOptions(desc, &a_opt);
461
462 po::variables_map vm;
463 po::store(po::parse_command_line(argc, argv, desc), vm);
464 po::notify(vm);
465
466 if (vm.count("help") != 0u)
467 {
468 std::cout << desc << std::endl;
469 return 1;
470 }
471
472 chainPlanning(output, space, planners, links, obstacles, extra, c_opt, a_opt, bench);
473
474 return 0;
475}
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
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...
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49