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