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 <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 {
63 
64  double zero = std::numeric_limits<double>::epsilon();
65  double eps = std::numeric_limits<double>::epsilon();
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 }
Tangent space and bounding polytope approximating some patch of the manifold.
Definition: AtlasChart.h:53
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
virtual State * geodesicInterpolate(const std::vector< State * > &geodesic, double t) const
Like interpolate(...), but interpolates between intermediate states already supplied in stateList fro...
void constrainedSanityChecks(unsigned int flags) const
Do some sanity checks relating to discrete geodesic computation and constraint satisfaction....
@ CONSTRAINED_STATESPACE_SAMPLERS
Check whether state samplers return constraint satisfying samples.
@ CONSTRAINED_STATESPACE_GEODESIC_CONTINUITY
Check whether discrete geodesics keep lambda_ * delta_ continuity.
A shared pointer wrapper for ompl::base::Constraint.
A shared pointer wrapper for ompl::base::StateSpace.
@ STATESPACE_DISTANCE_BOUND
Check whether the StateSpace::distance() is bounded by StateSpace::getExtent()
Definition: StateSpace.h:152
@ STATESPACE_DISTANCE_DIFFERENT_STATES
Check whether the distances between non-equal states is strictly positive (StateSpace::distance())
Definition: StateSpace.h:139
@ STATESPACE_RESPECT_BOUNDS
Check whether sampled states are always within bounds.
Definition: StateSpace.h:155
@ STATESPACE_DISTANCE_SYMMETRIC
Check whether the distance function is symmetric (StateSpace::distance())
Definition: StateSpace.h:142
@ STATESPACE_ENFORCE_BOUNDS_NO_OP
Check that enforceBounds() does not modify the contents of states that are within bounds.
Definition: StateSpace.h:158
virtual void sanityChecks() const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
Definition: StateSpace.cpp:603
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
void sanityChecks() const override
Do sanity checks, minus geodesic constraint satisfiability (as this is a lazy method).
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...
State * geodesicInterpolate(const std::vector< State * > &geodesic, double t) const override
Like interpolate(...), but interpolates between intermediate states already supplied in stateList fro...
bool project(State *state) const
Reproject a state onto the surface of the manifold. Returns true if projection was successful,...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64