TangentBundleStateSpace.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: Zachary Kingston */
36 
37 #include "ompl/base/spaces/constraint/TangentBundleStateSpace.h"
38 #include "ompl/base/spaces/constraint/AtlasChart.h"
39 
40 #include "ompl/util/Exception.h"
41 
42 #include <eigen3/Eigen/Core>
43 
44 #include <cmath>
45 
46 ompl::base::TangentBundleStateSpace::TangentBundleStateSpace(const StateSpacePtr &ambientSpace,
47  const ConstraintPtr &constraint)
48  : AtlasStateSpace(ambientSpace, constraint, false)
49 {
50  setName("TangentBundle" + space_->getName());
51  setBiasFunction([&](AtlasChart *c) -> double {
52  double d = 0;
53  for (auto anchor : anchors_)
54  d = std::max(d, distance(anchor, c->getOrigin()));
55 
56  return d;
57  });
58 }
59 
61 {
62  constrainedSanityChecks(CONSTRAINED_STATESPACE_GEODESIC_CONTINUITY | CONSTRAINED_STATESPACE_SAMPLERS);
63 
64  double zero = std::numeric_limits<double>::epsilon();
65  double eps = std::numeric_limits<double>::epsilon();
66  unsigned int flags = STATESPACE_DISTANCE_DIFFERENT_STATES | STATESPACE_DISTANCE_SYMMETRIC |
67  STATESPACE_DISTANCE_BOUND | STATESPACE_RESPECT_BOUNDS | STATESPACE_ENFORCE_BOUNDS_NO_OP;
68 
69  StateSpace::sanityChecks(zero, eps, flags);
70 }
71 
72 bool ompl::base::TangentBundleStateSpace::discreteGeodesic(const State *from, const State *to, bool interpolate,
73  std::vector<State *> *geodesic) const
74 {
75  // We can't traverse the manifold if we don't start on it.
76  if (!constraint_->isSatisfied(from))
77  return false;
78 
79  auto afrom = from->as<StateType>();
80  auto ato = to->as<StateType>();
81 
82  // Try to get starting chart from `from` state.
83  AtlasChart *c = getChart(afrom);
84  if (c == nullptr)
85  return false;
86 
87  // Save a copy of the from state.
88  if (geodesic != nullptr)
89  {
90  geodesic->clear();
91  geodesic->push_back(cloneState(from));
92  }
93 
94  auto &&svc = si_->getStateValidityChecker();
95 
96  // No need to traverse the manifold if we are already there
97  const double tolerance = delta_;
98  const double distTo = distance(from, to);
99  if (distTo <= tolerance)
100  return true;
101 
102  // Traversal stops if the ball of radius distMax centered at from is left
103  const double distMax = lambda_ * distTo;
104 
105  // Create a scratch state to use for movement.
106  auto scratch = cloneState(from)->as<StateType>();
107  auto temp = cloneState(from)->as<StateType>();
108 
109  // Project from and to points onto the chart
110  Eigen::VectorXd u_j(k_), u_b(k_);
111  c->psiInverse(*scratch, u_j);
112  c->psiInverse(*ato, u_b);
113 
114  bool done = false;
115  std::size_t chartsCreated = 0;
116  double dist = 0;
117 
118  const double sqDelta = delta_ * delta_;
119  do
120  {
121  // Take a step towards the final state
122  u_j += delta_ * (u_b - u_j).normalized();
123  c->phi(u_j, *temp);
124 
125  const double step = distance(temp, scratch);
126 
127  if (step < std::numeric_limits<double>::epsilon())
128  break;
129 
130  dist += step;
131 
132  if (!(interpolate || svc->isValid(scratch)) // valid
133  || distance(temp, from) > distMax || !std::isfinite(dist) // exceed max dist
134  || dist > distMax // exceed wandering
135  || chartsCreated > maxChartsPerExtension_) // exceed chart limit
136  break;
137 
138  done = (u_b - u_j).squaredNorm() <= sqDelta;
139  // Find or make a new chart if new state is off of current chart
140  if (done || !c->inPolytope(u_j) // outside polytope
141  || constraint_->distance(*temp) > epsilon_) // to far from manifold
142  {
143  const bool onManifold = c->psi(u_j, *temp);
144  if (!onManifold)
145  break;
146 
147  copyState(scratch, temp);
148  scratch->setChart(c);
149 
150  bool created = false;
151  if ((c = getChart(scratch, true, &created)) == nullptr)
152  {
153  OMPL_ERROR("Treating singularity as an obstacle.");
154  break;
155  }
156  chartsCreated += created;
157 
158  // Re-project onto the next chart.
159  c->psiInverse(*scratch, u_j);
160  c->psiInverse(*ato, u_b);
161 
162  done = (u_b - u_j).squaredNorm() <= sqDelta;
163  }
164 
165  copyState(scratch, temp);
166 
167  // Keep the state in a list, if requested.
168  if (geodesic != nullptr)
169  geodesic->push_back(cloneState(scratch));
170 
171  } while (!done);
172 
173  const bool ret = distance(to, scratch) <= delta_;
174  freeState(scratch);
175  freeState(temp);
176 
177  return ret;
178 }
179 
181  const double t) const
182 {
183  auto state = ConstrainedStateSpace::geodesicInterpolate(geodesic, t)->as<StateType>();
184  if (!project(state))
185  return geodesic[0];
186 
187  return state;
188 }
189 
191 {
192  auto astate = state->as<StateType>();
193  auto &&svc = si_->getStateValidityChecker();
194 
195  Eigen::VectorXd u(k_);
196  AtlasChart *chart = getChart(astate, true);
197  chart->psiInverse(*astate, u);
198 
199  if (chart->psi(u, *astate) // On manifold
200  && svc->isValid(state)) // Valid state
201  return true;
202 
203  return false;
204 }
State * geodesicInterpolate(const std::vector< State *> &geodesic, double t) const override
Like interpolate(...), but interpolates between intermediate states already supplied in stateList fro...
Tangent space and bounding polytope approximating some patch of the manifold.
Definition: AtlasChart.h:52
bool project(State *state) const
Reproject a state onto the surface of the manifold. Returns true if projection was successful...
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
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void sanityChecks() const override
Do sanity checks, minus geodesic constraint satisfiability (as this is a lazy method).
Definition of an abstract state.
Definition: State.h:49
virtual void sanityChecks() const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
virtual State * geodesicInterpolate(const std::vector< State *> &geodesic, double t) const
Like interpolate(...), but interpolates between intermediate states already supplied in stateList fro...
bool discreteGeodesic(const State *from, const State *to, bool interpolate=false, std::vector< State *> *geodesic=nullptr) const override
Traverse the manifold from from toward to. Returns true if we reached to, and false if we stopped ear...
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...