AtlasChart.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, 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, Caleb Voss */
36 
37 #ifndef OMPL_BASE_SPACES_ATLAS_CHART_
38 #define OMPL_BASE_SPACES_ATLAS_CHART_
39 
40 #include "ompl/base/spaces/constraint/AtlasStateSpace.h"
41 #include "ompl/datastructures/PDF.h"
42 
43 #include <vector>
44 #include <eigen3/Eigen/Core>
45 
46 namespace ompl
47 {
48  namespace base
49  {
52  class AtlasChart
53  {
54  private:
59  class Halfspace
60  {
61  public:
62  // non-copyable
63  Halfspace(const Halfspace &) = delete;
64  Halfspace &operator=(const Halfspace &) = delete;
65 
69  Halfspace(const AtlasChart *owner, const AtlasChart *neighbor);
70 
73  bool contains(const Eigen::Ref<const Eigen::VectorXd> &v) const;
74 
79  void checkNear(const Eigen::Ref<const Eigen::VectorXd> &v) const;
80 
86  bool circleIntersect(double r, Eigen::Ref<Eigen::VectorXd> v1, Eigen::Ref<Eigen::VectorXd> v2) const;
87 
91  static void intersect(const Halfspace &l1, const Halfspace &l2, Eigen::Ref<Eigen::VectorXd> out);
92 
95  void setComplement(Halfspace *complement)
96  {
97  complement_ = complement;
98  }
99 
101  Halfspace *getComplement() const
102  {
103  return complement_;
104  }
105 
107  const AtlasChart *getOwner() const
108  {
109  return owner_;
110  }
111 
112  private:
114  const AtlasChart *owner_;
115 
118  Halfspace *complement_{nullptr};
119 
122  Eigen::VectorXd u_;
123 
125  double usqnorm_;
126 
128  double rhs_;
129 
132  void setU(const Eigen::Ref<const Eigen::VectorXd> &u);
133 
138  double distanceToPoint(const Eigen::Ref<const Eigen::VectorXd> &v) const;
139 
142  void expandToInclude(const Eigen::Ref<const Eigen::VectorXd> &x);
143  };
144 
145  public:
146  // non-copyable
147  AtlasChart(const AtlasChart &) = delete;
148  AtlasChart &operator=(const AtlasChart &) = delete;
149 
153  AtlasChart(const AtlasStateSpace *atlas, const AtlasStateSpace::StateType *state);
154 
156  ~AtlasChart();
157 
160  void clear();
161 
165  {
166  return state_;
167  }
168 
170  unsigned int getAmbientDimension() const
171  {
172  return n_;
173  }
174 
176  unsigned int getManifoldDimension() const
177  {
178  return k_;
179  }
180 
185  void phi(const Eigen::Ref<const Eigen::VectorXd> &u, Eigen::Ref<Eigen::VectorXd> out) const;
186 
190  bool psi(const Eigen::Ref<const Eigen::VectorXd> &u, Eigen::Ref<Eigen::VectorXd> out) const;
191 
195  void psiInverse(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const;
196 
200  bool inPolytope(const Eigen::Ref<const Eigen::VectorXd> &u, const Halfspace *ignore1 = nullptr,
201  const Halfspace *ignore2 = nullptr) const;
202 
206  void borderCheck(const Eigen::Ref<const Eigen::VectorXd> &v) const;
207 
210  const AtlasChart *owningNeighbor(const Eigen::Ref<const Eigen::VectorXd> &x) const;
211 
217  bool toPolygon(std::vector<Eigen::VectorXd> &vertices) const;
218 
221  std::size_t getNeighborCount() const
222  {
223  return polytope_.size();
224  }
225 
229  bool estimateIsFrontier() const;
230 
235  static void generateHalfspace(AtlasChart *c1, AtlasChart *c2);
236 
237  protected:
240 
242  std::vector<Halfspace *> polytope_;
243 
247  void addBoundary(Halfspace *halfspace);
248 
249  private:
251  const unsigned int n_;
252 
255  const unsigned int k_;
256 
258  const AtlasStateSpace::StateType *state_;
259 
261  const Eigen::MatrixXd bigPhi_;
262 
264  const double radius_;
265  };
266  }
267 }
268 
269 #endif
void borderCheck(const Eigen::Ref< const Eigen::VectorXd > &v) const
Check if chart point v lies very close to any part of the boundary. Wherever it does, expand the neighboring chart&#39;s boundary to include.
Definition: AtlasChart.cpp:256
unsigned int getAmbientDimension() const
Returns the dimension of the ambient space.
Definition: AtlasChart.h:170
ConstrainedStateSpace encapsulating a planner-agnostic atlas algorithm for planning on a constraint m...
unsigned int getManifoldDimension() const
Returns the dimension of the manifold.
Definition: AtlasChart.h:176
const Constraint * constraint_
The constraint function that defines the manifold.
Definition: AtlasChart.h:239
A state in an atlas represented as a real vector in ambient space and a chart that it belongs to...
Tangent space and bounding polytope approximating some patch of the manifold.
Definition: AtlasChart.h:52
bool inPolytope(const Eigen::Ref< const Eigen::VectorXd > &u, const Halfspace *ignore1=nullptr, const Halfspace *ignore2=nullptr) const
Check if a point u on the chart lies within its polytope boundary. Can ignore up to 2 of the halfspac...
Definition: AtlasChart.cpp:238
bool estimateIsFrontier() const
Use sampling to make a quick estimate as to whether this chart&#39;s polytope boundary is completely defi...
Definition: AtlasChart.cpp:352
bool psi(const Eigen::Ref< const Eigen::VectorXd > &u, Eigen::Ref< Eigen::VectorXd > out) const
Exponential mapping. Project chart point u onto the manifold and store the result in out...
Definition: AtlasChart.cpp:190
void psiInverse(const Eigen::Ref< const Eigen::VectorXd > &x, Eigen::Ref< Eigen::VectorXd > out) const
Logarithmic mapping. Project ambient point x onto the chart and store the result in out...
Definition: AtlasChart.cpp:232
~AtlasChart()
Destructor.
Definition: AtlasChart.cpp:172
std::vector< Halfspace * > polytope_
Set of halfspaces defining the polytope boundary.
Definition: AtlasChart.h:242
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void phi(const Eigen::Ref< const Eigen::VectorXd > &u, Eigen::Ref< Eigen::VectorXd > out) const
Rewrite a chart point u in ambient space coordinates and store the result in out, which should be all...
Definition: AtlasChart.cpp:185
const AtlasStateSpace::StateType * getOrigin() const
Returns phi(0), the center of the chart in ambient space.
Definition: AtlasChart.h:164
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:75
void clear()
Forget all acquired information such as the halfspace boundary.
Definition: AtlasChart.cpp:177
static void generateHalfspace(AtlasChart *c1, AtlasChart *c2)
Create two complementary halfspaces dividing the space between charts c1 and c2, and add them to the ...
Definition: AtlasChart.cpp:369
const AtlasChart * owningNeighbor(const Eigen::Ref< const Eigen::VectorXd > &x) const
Try to find an owner for ambient point from among the neighbors of this chart. Returns nullptr if no...
Definition: AtlasChart.cpp:262
void addBoundary(Halfspace *halfspace)
Introduce a new halfspace to the chart&#39;s bounding polytope. This chart assumes responsibility for del...
Definition: AtlasChart.cpp:387
bool toPolygon(std::vector< Eigen::VectorXd > &vertices) const
For manifolds of dimension 2, return in order in vertices the polygon boundary of this chart...
Definition: AtlasChart.cpp:283
std::size_t getNeighborCount() const
Returns the number of charts this chart shares a halfspace boundary with.
Definition: AtlasChart.h:221