ConstrainedPlanningKinematicChain.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 "../KinematicChain.h"
38 #include "ConstrainedPlanningCommon.h"
39 
40 class ConstrainedKinematicChainValidityChecker : public KinematicChainValidityChecker
41 {
42 public:
43  ConstrainedKinematicChainValidityChecker(const ob::ConstrainedSpaceInformationPtr &si)
45  {
46  }
47 
48  bool isValid(const ob::State *state) const override
49  {
50  auto &&space = si_->getStateSpace()->as<ob::ConstrainedStateSpace>()->getSpace()->as<KinematicChainSpace>();
51  auto &&s = state->as<ob::ConstrainedStateSpace::StateType>()->getState()->as<KinematicChainSpace::StateType>();
52  return isValidImpl(space, s);
53  }
54 };
55 
56 class KinematicChainConstraint : public ob::Constraint
57 {
58 public:
59  KinematicChainConstraint(unsigned int links, double linkLength) : ob::Constraint(links, 1), linkLength_(linkLength)
60  {
61  }
62 
63  void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
64  {
65  Eigen::Vector2d e = Eigen::Vector2d::Zero(), eN = Eigen::Vector2d::Zero();
66  double theta = 0.;
67 
68  for (unsigned int i = 0; i < x.size(); ++i)
69  {
70  theta += x[i];
71  eN[0] = e[0] + cos(theta) * linkLength_;
72  eN[1] = e[1] + sin(theta) * linkLength_;
73  e = eN;
74  }
75 
76  out[0] = e[1] - linkLength_;
77  }
78 
79 private:
80  double linkLength_;
81 };
82 
83 bool chainPlanningOnce(ConstrainedProblem &cp, enum PLANNER_TYPE planner, bool output)
84 {
85  cp.setPlanner(planner);
86 
87  // Solve the problem
88  ob::PlannerStatus stat = cp.solveOnce(true);
89 
90  if (output && stat)
91  {
92  auto filename = boost::str(boost::format("kinematic_path_%i.dat") % cp.constraint->getAmbientDimension());
93  OMPL_INFORM("Dumping problem information to `%s`.", filename.c_str());
94  auto path = cp.ss->getSolutionPath();
95  path.interpolate();
96  std::ofstream pathfile(filename);
97  path.printAsMatrix(pathfile);
98  pathfile.close();
99  }
100 
101  cp.atlasStats();
102 
103  return stat;
104 }
105 
106 bool chainPlanningBench(ConstrainedProblem &cp, std::vector<enum PLANNER_TYPE> &planners)
107 {
108  cp.setupBenchmark(planners, "kinematic");
109  cp.bench->addExperimentParameter("links", "INTEGER", std::to_string(cp.constraint->getAmbientDimension()));
110 
111  cp.runBenchmark();
112  return false;
113 }
114 
115 bool chainPlanning(bool output, enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners, unsigned int links,
116  struct ConstrainedOptions &c_opt, struct AtlasOptions &a_opt, bool bench)
117 {
118  Environment env = createEmptyEnvironment(links);
119 
120  // Create the ambient space state space for the problem.
121  auto ss = std::make_shared<KinematicChainSpace>(links, 1. / (double)links, &env);
122 
123  // Create a shared pointer to our constraint.
124  auto constraint = std::make_shared<KinematicChainConstraint>(links, 1. / (double)links);
125 
126  ConstrainedProblem cp(space, ss, constraint);
127  cp.setConstrainedOptions(c_opt);
128  cp.setAtlasOptions(a_opt);
129 
130  Eigen::VectorXd start, goal;
131  start = Eigen::VectorXd::Constant(links, 0);
132  goal = Eigen::VectorXd::Constant(links, 0);
133 
134  start[links - 1] = boost::math::constants::pi<double>() / 2;
135  goal[0] = boost::math::constants::pi<double>();
136  goal[links - 1] = -boost::math::constants::pi<double>() / 2;
137 
138  cp.setStartAndGoalStates(start, goal);
139  cp.ss->setStateValidityChecker(std::make_shared<ConstrainedKinematicChainValidityChecker>(cp.csi));
140 
141  if (!bench)
142  return chainPlanningOnce(cp, planners[0], output);
143  else
144  return chainPlanningBench(cp, planners);
145 }
146 
147 auto help_msg = "Shows this help message.";
148 auto output_msg = "Dump found solution path (if one exists) and environment to data files that can be rendered with "
149  "`KinematicChainPathPlot.py`.";
150 auto links_msg = "Number of links in kinematic chain.";
151 auto bench_msg = "Do benchmarking on provided planner list.";
152 
153 int main(int argc, char **argv)
154 {
155  bool output, bench;
156  enum SPACE_TYPE space = PJ;
157  std::vector<enum PLANNER_TYPE> planners = {RRT};
158 
159  struct ConstrainedOptions c_opt;
160  struct AtlasOptions a_opt;
161 
162  unsigned int links;
163 
164  po::options_description desc("Options");
165  desc.add_options()("help,h", help_msg);
166  desc.add_options()("output,o", po::bool_switch(&output)->default_value(false), output_msg);
167  desc.add_options()("links,l", po::value<unsigned int>(&links)->default_value(5), links_msg);
168  desc.add_options()("bench", po::bool_switch(&bench)->default_value(false), bench_msg);
169 
170  addSpaceOption(desc, &space);
171  addPlannerOption(desc, &planners);
172  addConstrainedOptions(desc, &c_opt);
173  addAtlasOptions(desc, &a_opt);
174 
175  po::variables_map vm;
176  po::store(po::parse_command_line(argc, argv, desc), vm);
177  po::notify(vm);
178 
179  if (vm.count("help") != 0u)
180  {
181  std::cout << desc << std::endl;
182  return 1;
183  }
184 
185  return static_cast<int>(chainPlanning(output, space, planners, links, c_opt, a_opt, bench));
186 }
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
bool isValid(const ompl::base::State *state) const override
Return true if the state state is valid. Usually, this means at least collision checking....
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()
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:151
SpaceInformation * si_
The instance of space information this state validity checker operates on.
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.