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
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
369void ompl::base::AtlasChart::generateHalfspace(AtlasChart *c1, AtlasChart *c2)
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...
double gaussian01()
Generate a random real using a normal distribution with mean 0 and variance 1.
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
static void generateHalfspace(AtlasChart *c1, AtlasChart *c2)
Create two complementary halfspaces dividing the space between charts c1 and c2, and add them to the ...
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...
double getRho_s() const
Get the sampling radius.