Loading...
Searching...
No Matches
AtlasChart.cpp
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: Caleb Voss */
36
37#include "ompl/base/spaces/constraint/AtlasChart.h"
38#include <boost/math/constants/constants.hpp>
39#include <Eigen/Dense>
40
42
44
45ompl::base::AtlasChart::Halfspace::Halfspace(const AtlasChart *owner, const AtlasChart *neighbor) : owner_(owner)
46{
47 // Project neighbor's chart center onto our chart.
48 Eigen::VectorXd u(owner_->k_);
49 owner_->psiInverse(*neighbor->getOrigin(), u);
50
51 // Compute the halfspace equation, which is the perpendicular bisector
52 // between 0 and u (plus 5% to reduce cracks, see Jaillet et al.).
53 setU(1.05 * u);
54}
55
56bool ompl::base::AtlasChart::Halfspace::contains(const Eigen::Ref<const Eigen::VectorXd> &v) const
57{
58 return v.dot(u_) <= rhs_;
59}
60
61void ompl::base::AtlasChart::Halfspace::checkNear(const Eigen::Ref<const Eigen::VectorXd> &v) const
62{
63 // Threshold is 10% of the distance from the boundary to the origin.
64 if (distanceToPoint(v) < 1.0 / 20)
65 {
66 Eigen::VectorXd x(owner_->n_);
67 owner_->psi(v, x);
68 complement_->expandToInclude(x);
69 }
70}
71
72bool ompl::base::AtlasChart::Halfspace::circleIntersect(const double r, Eigen::Ref<Eigen::VectorXd> v1,
73 Eigen::Ref<Eigen::VectorXd> v2) const
74{
75 if (owner_->getManifoldDimension() != 2)
76 throw ompl::Exception("ompl::base::AtlasChart::Halfspace::circleIntersect() "
77 "Only works on 2D manifolds.");
78
79 // Check if there will be no solutions.
80 double discr = 4 * r * r - usqnorm_;
81 if (discr < 0)
82 return false;
83 discr = std::sqrt(discr);
84
85 // Compute the 2 solutions (possibly 1 repeated solution).
86 double unorm = std::sqrt(usqnorm_);
87 v1[0] = -u_[1] * discr;
88 v1[1] = u_[0] * discr;
89 v2 = -v1;
90 v1 += u_ * unorm;
91 v2 += u_ * unorm;
92 v1 /= 2 * unorm;
93 v2 /= 2 * unorm;
94
95 return true;
96}
97
99
100void ompl::base::AtlasChart::Halfspace::intersect(const Halfspace &l1, const Halfspace &l2,
101 Eigen::Ref<Eigen::VectorXd> out)
102{
103 if (l1.owner_ != l2.owner_)
104 throw ompl::Exception("Cannot intersect linear inequalities on different charts.");
105 if (l1.owner_->getManifoldDimension() != 2)
106 throw ompl::Exception("AtlasChart::Halfspace::intersect() only works on 2D manifolds.");
107
108 // Computer the intersection point of these lines.
109 Eigen::MatrixXd A(2, 2);
110 A.row(0) = l1.u_.transpose();
111 A.row(1) = l2.u_.transpose();
112 out[0] = l1.u_.squaredNorm();
113 out[1] = l2.u_.squaredNorm();
114 out = 0.5 * A.inverse() * out;
115}
116
118
119void ompl::base::AtlasChart::Halfspace::setU(const Eigen::Ref<const Eigen::VectorXd> &u)
120{
121 u_ = u;
122
123 // Precompute the squared norm of u.
124 usqnorm_ = u_.squaredNorm();
125
126 // Precompute the right-hand side of the linear inequality.
127 rhs_ = usqnorm_ / 2;
128}
129
130double ompl::base::AtlasChart::Halfspace::distanceToPoint(const Eigen::Ref<const Eigen::VectorXd> &v) const
131{
132 // Result is a scalar factor of u_.
133 return (0.5 - v.dot(u_)) / usqnorm_;
134}
135
136void ompl::base::AtlasChart::Halfspace::expandToInclude(const Eigen::Ref<const Eigen::VectorXd> &x)
137{
138 // Compute how far v = psiInverse(x) lies past the boundary, if at all.
139 Eigen::VectorXd v(owner_->k_);
140 owner_->psiInverse(x, v);
141 const double t = -distanceToPoint(v);
142
143 // Move u_ further out by twice that much.
144 if (t > 0)
145 setU((1 + 2 * t) * u_);
146}
147
149
151
152ompl::base::AtlasChart::AtlasChart(const AtlasStateSpace *atlas, const AtlasStateSpace::StateType *state)
153 : constraint_(atlas->getConstraint().get())
154 , n_(atlas->getAmbientDimension())
155 , k_(atlas->getManifoldDimension())
156 , state_(state)
157 , bigPhi_([&]() -> const Eigen::MatrixXd {
158 Eigen::MatrixXd j(n_ - k_, n_);
159 constraint_->jacobian(*state_, j);
160
161 Eigen::FullPivLU<Eigen::MatrixXd> decomp = j.fullPivLu();
162 if (!decomp.isSurjective())
163 throw ompl::Exception("Cannot compute full-rank tangent space.");
164
165 // Compute the null space and orthonormalize, which is a basis for the tangent space.
166 return decomp.kernel().householderQr().householderQ() * Eigen::MatrixXd::Identity(n_, k_);
167 }())
168 , radius_(atlas->getRho_s())
169{
170}
171
173{
174 clear();
175}
176
178{
179 for (auto h : polytope_)
180 delete h;
181
182 polytope_.clear();
183}
184
185void ompl::base::AtlasChart::phi(const Eigen::Ref<const Eigen::VectorXd> &u, Eigen::Ref<Eigen::VectorXd> out) const
186{
187 out = *state_ + bigPhi_ * u;
188}
189
190bool ompl::base::AtlasChart::psi(const Eigen::Ref<const Eigen::VectorXd> &u, Eigen::Ref<Eigen::VectorXd> out) const
191{
192 // Initial guess for Newton's method
193 Eigen::VectorXd x0(n_);
194 phi(u, x0);
195
196 // Newton-Raphson to solve Ax = b
197 unsigned int iter = 0;
198 double norm = 0;
199 Eigen::MatrixXd A(n_, n_);
200 Eigen::VectorXd b(n_);
201
202 const double tolerance = constraint_->getTolerance();
203 const double squaredTolerance = tolerance * tolerance;
204
205 // Initialize output to initial guess
206 out = x0;
207
208 // Initialize A with orthonormal basis (constant)
209 A.block(n_ - k_, 0, k_, n_) = bigPhi_.transpose();
210
211 // Initialize b with initial f(out) = b
212 constraint_->function(out, b.head(n_ - k_));
213 b.tail(k_).setZero();
214
215 while ((norm = b.squaredNorm()) > squaredTolerance && iter++ < constraint_->getMaxIterations())
216 {
217 // Recompute the Jacobian at the new guess.
218 constraint_->jacobian(out, A.block(0, 0, n_ - k_, n_));
219
220 // Move in the direction that decreases F(out) and is perpendicular to
221 // the chart.
222 out -= A.partialPivLu().solve(b);
223
224 // Recompute b with new guess.
225 constraint_->function(out, b.head(n_ - k_));
226 b.tail(k_) = bigPhi_.transpose() * (out - x0);
227 }
228
229 return norm < squaredTolerance;
230}
231
232void ompl::base::AtlasChart::psiInverse(const Eigen::Ref<const Eigen::VectorXd> &x,
233 Eigen::Ref<Eigen::VectorXd> out) const
234{
235 out = bigPhi_.transpose() * (x - *state_);
236}
237
238bool ompl::base::AtlasChart::inPolytope(const Eigen::Ref<const Eigen::VectorXd> &u, const Halfspace *const ignore1,
239 const Halfspace *const ignore2) const
240{
241 if (u.norm() > radius_)
242 return false;
243
244 for (Halfspace *h : polytope_)
245 {
246 if (h == ignore1 || h == ignore2)
247 continue;
248
249 if (!h->contains(u))
250 return false;
251 }
252
253 return true;
254}
255
256void ompl::base::AtlasChart::borderCheck(const Eigen::Ref<const Eigen::VectorXd> &v) const
257{
258 for (Halfspace *h : polytope_)
259 h->checkNear(v);
260}
261
262const ompl::base::AtlasChart *ompl::base::AtlasChart::owningNeighbor(const Eigen::Ref<const Eigen::VectorXd> &x) const
263{
264 Eigen::VectorXd projx(n_), proju(k_);
265 for (Halfspace *h : polytope_)
266 {
267 // Project onto the neighboring chart.
268 const AtlasChart *c = h->getComplement()->getOwner();
269 c->psiInverse(x, proju);
270 c->phi(proju, projx);
271
272 // Check if it's within the validity region and polytope boundary.
273 const bool withinTolerance = (projx - x).norm();
274 const bool inPolytope = c->inPolytope(proju);
275
276 if (withinTolerance && inPolytope)
277 return c;
278 }
279
280 return nullptr;
281}
282
283bool ompl::base::AtlasChart::toPolygon(std::vector<Eigen::VectorXd> &vertices) const
284{
285 if (k_ != 2)
286 throw ompl::Exception("AtlasChart::toPolygon() only works on 2D manifold/charts.");
287
288 // Compile a list of all the vertices in P and all the times the border
289 // intersects the circle.
290 Eigen::VectorXd v(2);
291 Eigen::VectorXd intersection(n_);
292 vertices.clear();
293 for (std::size_t i = 0; i < polytope_.size(); i++)
294 {
295 for (std::size_t j = i + 1; j < polytope_.size(); j++)
296 {
297 // Check if intersection of the lines is a part of the boundary and
298 // within the circle.
299 Halfspace::intersect(*polytope_[i], *polytope_[j], v);
300 phi(v, intersection);
301 if (inPolytope(v, polytope_[i], polytope_[j]))
302 vertices.push_back(intersection);
303 }
304
305 // Check if intersection with circle is part of the boundary.
306 Eigen::VectorXd v1(2), v2(2);
307 if ((polytope_[i])->circleIntersect(radius_, v1, v2))
308 {
309 if (inPolytope(v1, polytope_[i]))
310 {
311 phi(v1, intersection);
312 vertices.push_back(intersection);
313 }
314 if (inPolytope(v2, polytope_[i]))
315 {
316 phi(v2, intersection);
317 vertices.push_back(intersection);
318 }
319 }
320 }
321
322 // Include points approximating the circle, if they're inside the polytope.
323 bool is_frontier = false;
324 Eigen::VectorXd v0(2);
325 v0 << radius_, 0;
326 const double step = boost::math::constants::pi<double>() / 32.;
327 for (double a = 0.; a < 2. * boost::math::constants::pi<double>(); a += step)
328 {
329 const Eigen::VectorXd vn = Eigen::Rotation2Dd(a) * v0;
330
331 if (inPolytope(vn))
332 {
333 is_frontier = true;
334 phi(vn, intersection);
335 vertices.push_back(intersection);
336 }
337 }
338
339 // Put all the points in order.
340 std::sort(vertices.begin(), vertices.end(),
341 [&](const Eigen::Ref<const Eigen::VectorXd> &x1, const Eigen::Ref<const Eigen::VectorXd> &x2) -> bool {
342 // Check the angles to see who should come first.
343 Eigen::VectorXd v1(2), v2(2);
344 psiInverse(x1, v1);
345 psiInverse(x2, v2);
346 return std::atan2(v1[1], v1[0]) < std::atan2(v2[1], v2[0]);
347 });
348
349 return is_frontier;
350}
351
353{
354 RNG rng;
355 Eigen::VectorXd ru(k_);
356 for (int k = 0; k < 1000; k++)
357 {
358 for (int i = 0; i < ru.size(); i++)
359 ru[i] = rng.gaussian01();
360 ru *= radius_ / ru.norm();
361 if (inPolytope(ru))
362 return true;
363 }
364 return false;
365}
366
368
370{
371 if (c1 == c2)
372 throw ompl::Exception("ompl::base::AtlasChart::generateHalfspace(): "
373 "Must use two different charts.");
374
375 // c1, c2 will delete l1, l2, respectively, upon destruction.
376 Halfspace *l1, *l2;
377 l1 = new Halfspace(c1, c2);
378 l2 = new Halfspace(c2, c1);
379 l1->setComplement(l2);
380 l2->setComplement(l1);
381 c1->addBoundary(l1);
382 c2->addBoundary(l2);
383}
384
386
387void ompl::base::AtlasChart::addBoundary(Halfspace *halfspace)
388{
389 polytope_.push_back(halfspace);
390}
The exception type for ompl.
Definition: Exception.h:47
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
double gaussian01()
Generate a random real using a normal distribution with mean 0 and variance 1.
Definition: RandomNumbers.h:93
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...
Definition: AtlasChart.cpp:238
bool estimateIsFrontier() const
Use sampling to make a quick estimate as to whether this chart's polytope boundary is completely defi...
Definition: AtlasChart.cpp:352
void addBoundary(Halfspace *halfspace)
Introduce a new halfspace to the chart's bounding polytope. This chart assumes responsibility for del...
Definition: AtlasChart.cpp:387
void clear()
Forget all acquired information such as the halfspace boundary.
Definition: AtlasChart.cpp:177
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
~AtlasChart()
Destructor.
Definition: AtlasChart.cpp:172
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,...
Definition: AtlasChart.cpp:256
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
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
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
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 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 ...
Definition: AtlasChart.cpp:262
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...
double getRho_s() const
Get the sampling radius.