Loading...
Searching...
No Matches
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 <Eigen/Core>
45
46namespace 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
Tangent space and bounding polytope approximating some patch of the manifold.
Definition AtlasChart.h:53
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...
std::vector< Halfspace * > polytope_
Set of halfspaces defining the polytope boundary.
Definition AtlasChart.h:242
bool estimateIsFrontier() const
Use sampling to make a quick estimate as to whether this chart's polytope boundary is completely defi...
void addBoundary(Halfspace *halfspace)
Introduce a new halfspace to the chart's bounding polytope. This chart assumes responsibility for del...
void clear()
Forget all acquired information such as the halfspace boundary.
unsigned int getAmbientDimension() const
Returns the dimension of the ambient space.
Definition AtlasChart.h:170
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,...
unsigned int getManifoldDimension() const
Returns the dimension of the manifold.
Definition AtlasChart.h:176
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,...
bool toPolygon(std::vector< Eigen::VectorXd > &vertices) const
For manifolds of dimension 2, return in order in vertices the polygon boundary of this chart,...
const Constraint * constraint_
The constraint function that defines the manifold.
Definition AtlasChart.h:239
std::size_t getNeighborCount() const
Returns the number of charts this chart shares a halfspace boundary with.
Definition AtlasChart.h:221
static void generateHalfspace(AtlasChart *c1, AtlasChart *c2)
Create two complementary halfspaces dividing the space between charts c1 and c2, and add them to the ...
const AtlasStateSpace::StateType * getOrigin() const
Returns phi(0), the center of the chart in ambient space.
Definition AtlasChart.h:164
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,...
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...
const AtlasChart * owningNeighbor(const Eigen::Ref< const Eigen::VectorXd > &x) const
Try to find an owner for ambient point \x from among the neighbors of this chart. Returns nullptr if ...
A state in an atlas represented as a real vector in ambient space and a chart that it belongs to.
ConstrainedStateSpace encapsulating a planner-agnostic atlas algorithm for planning on a constraint m...
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition Constraint.h:76
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Main namespace. Contains everything in this library.