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 
45 ompl::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 
56 bool ompl::base::AtlasChart::Halfspace::contains(const Eigen::Ref<const Eigen::VectorXd> &v) const
57 {
58  return v.dot(u_) <= rhs_;
59 }
60 
61 void 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 
72 bool 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 
100 void 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 
119 void 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 
130 double 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 
136 void 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 
152 ompl::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 
185 void 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 
190 bool 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 
232 void 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 
238 bool 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 
256 void ompl::base::AtlasChart::borderCheck(const Eigen::Ref<const Eigen::VectorXd> &v) const
257 {
258  for (Halfspace *h : polytope_)
259  h->checkNear(v);
260 }
261 
262 const 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 
283 bool 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 
387 void 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.