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 
39 class ChainConstraint : public ob::Constraint
40 {
41 private:
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 
98 public:
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 
355 private:
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 
366 bool 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 
387 bool 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 
399 bool 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 
424 auto help_msg = "Shows this help message.";
425 auto 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";
427 auto links_msg = "Number of links in the kinematic chain. Minimum is 4.";
428 auto obstacles_msg = "Number of `wall' obstacles on the surface of the sphere. Ranges from [0, 2]";
429 auto 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";
434 auto bench_msg = "Do benchmarking on provided planner list.";
435 
436 int 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:107
Definition of an abstract state.
Definition: State.h:113
This namespace contains sampling based planning routines shared by both planning under geometric cons...
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
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
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:314
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
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
The lower and upper bounds for an Rn space.