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
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.
unsigned int maxChartsPerExtension_
Maximum number of charts that can be created in one manifold traversal.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
double epsilon_
Maximum distance between a chart and the manifold inside its validity region.
AtlasChart * getChart(const StateType *state, bool force=false, bool *created=nullptr) const
Wrapper to return chart state belongs to. Will attempt to initialize new chart if state does not belo...
virtual State * geodesicInterpolate(const std::vector< State * > &geodesic, double t) const
Like interpolate(...), but interpolates between intermediate states already supplied in stateList fro...
double lambda_
Manifold traversal from x to y is stopped if accumulated distance is greater than d(x,...
void interpolate(const State *from, const State *to, double t, State *state) const override
Find a state between from and to around time t, where t = 0 is from, and t = 1 is the final state rea...
double delta_
Step size when traversing the manifold and collision checking.
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.
const unsigned int k_
Manifold dimension.
SpaceInformation * si_
SpaceInformation associated with this space. Required for early collision checking in manifold traver...
const ConstraintPtr constraint_
Constraint function that defines the manifold.
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...
State * cloneState(const State *source) const
Clone a state.
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,...
void freeState(State *state) const override
Free the memory of the allocated state.
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...
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64