ConstrainedPlanningSphere.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 SphereConstraint : public ob::Constraint
40 {
41 public:
42  SphereConstraint() : ob::Constraint(3, 1)
43  {
44  }
45 
46  void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
47  {
48  out[0] = x.norm() - 1;
49  }
50 
51  void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
52  {
53  out = x.transpose().normalized();
54  }
55 };
56 
57 class SphereProjection : public ob::ProjectionEvaluator
58 {
59 public:
60  SphereProjection(const ob::StateSpacePtr &space) : ob::ProjectionEvaluator(space)
61  {
62  }
63 
64  unsigned int getDimension() const override
65  {
66  return 2;
67  }
68 
69  void defaultCellSizes() override
70  {
71  cellSizes_.resize(2);
72  cellSizes_[0] = 0.1;
73  cellSizes_[1] = 0.1;
74  }
75 
76  void project(const ob::State *state, Eigen::Ref<Eigen::VectorXd> projection) const override
77  {
78  auto &&x = *state->as<ob::ConstrainedStateSpace::StateType>();
79  projection(0) = atan2(x[1], x[0]);
80  projection(1) = acos(x[2]);
81  }
82 };
83 
84 bool obstacles(const ob::State *state)
85 {
86  auto &&x = *state->as<ob::ConstrainedStateSpace::StateType>();
87 
88  if (-0.80 < x[2] && x[2] < -0.6)
89  {
90  if (-0.05 < x[1] && x[1] < 0.05)
91  return x[0] > 0;
92  return false;
93  }
94  else if (-0.1 < x[2] && x[2] < 0.1)
95  {
96  if (-0.05 < x[0] && x[0] < 0.05)
97  return x[1] < 0;
98  return false;
99  }
100  else if (0.6 < x[2] && x[2] < 0.80)
101  {
102  if (-0.05 < x[1] && x[1] < 0.05)
103  return x[0] < 0;
104  return false;
105  }
106 
107  return true;
108 }
109 
110 bool spherePlanningOnce(ConstrainedProblem &cp, enum PLANNER_TYPE planner, bool output)
111 {
112  cp.setPlanner(planner, "sphere");
113 
114  // Solve the problem
115  ob::PlannerStatus stat = cp.solveOnce(output, "sphere");
116 
117  if (output)
118  {
119  OMPL_INFORM("Dumping problem information to `sphere_info.txt`.");
120  std::ofstream infofile("sphere_info.txt");
121  infofile << cp.type << std::endl;
122  infofile.close();
123  }
124 
125  cp.atlasStats();
126 
127  if (output)
128  cp.dumpGraph("sphere");
129 
130  return stat;
131 }
132 
133 bool spherePlanningBench(ConstrainedProblem &cp, std::vector<enum PLANNER_TYPE> &planners)
134 {
135  cp.setupBenchmark(planners, "sphere");
136  cp.runBenchmark();
137  return 0;
138 }
139 
140 bool spherePlanning(bool output, enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners,
141  struct ConstrainedOptions &c_opt, struct AtlasOptions &a_opt, bool bench)
142 {
143  // Create the ambient space state space for the problem.
144  auto rvss = std::make_shared<ob::RealVectorStateSpace>(3);
145 
146  ob::RealVectorBounds bounds(3);
147  bounds.setLow(-2);
148  bounds.setHigh(2);
149 
150  rvss->setBounds(bounds);
151 
152  // Create a shared pointer to our constraint.
153  auto constraint = std::make_shared<SphereConstraint>();
154 
155  ConstrainedProblem cp(space, rvss, constraint);
156  cp.setConstrainedOptions(c_opt);
157  cp.setAtlasOptions(a_opt);
158 
159  cp.css->registerProjection("sphere", std::make_shared<SphereProjection>(cp.css));
160 
161  Eigen::VectorXd start(3), goal(3);
162  start << 0, 0, -1;
163  goal << 0, 0, 1;
164 
165  cp.setStartAndGoalStates(start, goal);
166  cp.ss->setStateValidityChecker(obstacles);
167 
168  if (!bench)
169  return spherePlanningOnce(cp, planners[0], output);
170  else
171  return spherePlanningBench(cp, planners);
172 }
173 
174 auto help_msg = "Shows this help message.";
175 auto output_msg = "Dump found solution path (if one exists) in plain text and planning graph in GraphML to "
176  "`sphere_path.txt` and `sphere_graph.graphml` respectively.";
177 auto bench_msg = "Do benchmarking on provided planner list.";
178 
179 int main(int argc, char **argv)
180 {
181  bool output, bench;
182  enum SPACE_TYPE space = PJ;
183  std::vector<enum PLANNER_TYPE> planners = {RRT};
184 
185  struct ConstrainedOptions c_opt;
186  struct AtlasOptions a_opt;
187 
188  po::options_description desc("Options");
189  desc.add_options()("help,h", help_msg);
190  desc.add_options()("output,o", po::bool_switch(&output)->default_value(false), output_msg);
191  desc.add_options()("bench", po::bool_switch(&bench)->default_value(false), bench_msg);
192 
193  addSpaceOption(desc, &space);
194  addPlannerOption(desc, &planners);
195  addConstrainedOptions(desc, &c_opt);
196  addAtlasOptions(desc, &a_opt);
197 
198  po::variables_map vm;
199  po::store(po::parse_command_line(argc, argv, desc), vm);
200  po::notify(vm);
201 
202  if (vm.count("help"))
203  {
204  std::cout << desc << std::endl;
205  return 1;
206  }
207 
208  return spherePlanning(output, space, planners, c_opt, a_opt, bench);
209 }
virtual unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
A shared pointer wrapper for ompl::base::StateSpace.
virtual void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const =0
Compute the projection as an array of double values.
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
virtual void defaultCellSizes()
Set the default cell dimensions for this projection. The default implementation of this function is e...
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
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 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