Loading...
Searching...
No Matches
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
46ompl::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
72bool 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,...
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,...
A state in an atlas represented as a real vector in ambient space and a chart that it belongs to.
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 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
STL namespace.