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 }
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...
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
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
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:319
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
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.