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 <eigen3/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 }
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
double gaussian01()
Generate a random real using a normal distribution with mean 0 and variance 1.
Definition: RandomNumbers.h:92
ConstrainedStateSpace encapsulating a planner-agnostic atlas algorithm for planning on a constraint m...
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
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
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:56
void jacobian(const State *state, Eigen::Ref< Eigen::MatrixXd > out) const
Compute the Jacobian of the constraint function at state. Result is returned in out, which should be allocated to size coDim by ambientDim. Default implementation performs the differentiation numerically with a seven-point central difference stencil. It is best to provide an analytic formulation.
Definition: Constraint.cpp:45
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
The exception type for ompl.
Definition: Exception.h:46
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