ConstrainedStateSpace.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2017, 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/tools/config/MagicConstants.h"
38#include "ompl/base/spaces/constraint/ConstrainedStateSpace.h"
39#include "ompl/util/Exception.h"
40
42
44
46 : MotionValidator(si), ss_(*si->getStateSpace()->as<ConstrainedStateSpace>())
47{
48}
49
51 : MotionValidator(si), ss_(*si->getStateSpace()->as<ConstrainedStateSpace>())
52{
53}
54
56{
57 return ss_.getConstraint()->isSatisfied(s2) && ss_.discreteGeodesic(s1, s2, false);
58}
59
61 std::pair<State *, double> &lastValid) const
62{
63 // Invoke the manifold-traversing algorithm to save intermediate states
64 std::vector<ompl::base::State *> stateList;
65 bool reached = ss_.discreteGeodesic(s1, s2, false, &stateList);
66
67 // We are supposed to be able to assume that s1 is valid. However, it's not
68 // on rare occasions, and I don't know why. This makes stateList empty.
69 if (stateList.empty())
70 {
71 if (lastValid.first != nullptr)
72 ss_.copyState(lastValid.first, s1);
73
74 lastValid.second = 0;
75 return false;
76 }
77
78 double distanceTraveled = 0;
79 for (std::size_t i = 0; i < stateList.size() - 1; i++)
80 {
81 if (!reached)
82 distanceTraveled += ss_.distance(stateList[i], stateList[i + 1]);
83 ss_.freeState(stateList[i]);
84 }
85
86 if (!reached && (lastValid.first != nullptr))
87 {
88 // Check if manifold traversal stopped early and set its final state as
89 // lastValid.
90 ss_.copyState(lastValid.first, stateList.back());
91 // Compute the interpolation parameter of the last valid
92 // state. (Although if you then interpolate, you probably won't get this
93 // exact state back.)
94 double approxDistanceRemaining = ss_.distance(lastValid.first, s2);
95 lastValid.second = distanceTraveled / (distanceTraveled + approxDistanceRemaining);
96 }
97
98 ss_.freeState(stateList.back());
99 return ss_.getConstraint()->isSatisfied(s2) && reached;
100}
101
103 : WrapperStateSpace(space)
104 , constraint_(constraint)
105 , n_(space->getDimension())
106 , k_(constraint_->getManifoldDimension())
107{
108 setDelta(magic::CONSTRAINED_STATE_SPACE_DELTA);
109}
110
112{
113 auto *s1 = allocState()->as<StateType>();
114 State *s2 = allocState();
115 StateSamplerPtr ss = allocStateSampler();
116
117 bool isTraversable = false;
118
119 for (unsigned int i = 0; i < ompl::magic::TEST_STATE_COUNT; ++i)
120 {
121 bool satisfyGeodesics = false;
122 bool continuityGeodesics = false;
123
124 ss->sampleUniform(s1);
125
126 // Verify that the provided Jacobian routine for the constraint is close
127 // to the numerical approximation.
128 if (flags & CONSTRAINED_STATESPACE_JACOBIAN)
129 {
130 Eigen::MatrixXd j_a(n_ - k_, n_), j_n(n_ - k_, n_);
131
132 constraint_->jacobian(*s1, j_a); // Provided routine
133 constraint_->Constraint::jacobian(*s1, j_n); // Numerical approximation
134
135 if ((j_a - j_n).norm() > constraint_->getTolerance())
136 throw Exception("Constraint Jacobian deviates from numerical approximation.");
137 }
138
139 ss->sampleUniformNear(s2, s1, 10 * delta_);
140
141 // Check that samplers are returning constraint satisfying samples.
142 if (flags & CONSTRAINED_STATESPACE_SAMPLERS && (!constraint_->isSatisfied(s1) || !constraint_->isSatisfied(s2)))
143 throw Exception("State samplers generate constraint unsatisfying states.");
144
145 std::vector<State *> geodesic;
146 // Make sure that the manifold is traversable at least once.
147 if ((isTraversable |= discreteGeodesic(s1, s2, true, &geodesic)))
148 {
149 // Verify that geodesicInterpolate returns a constraint satisfying state.
150 if (flags & CONSTRAINED_STATESPACE_GEODESIC_INTERPOLATE &&
151 !constraint_->isSatisfied(geodesicInterpolate(geodesic, 0.5)))
152 throw Exception("Geodesic interpolate returns unsatisfying configurations.");
153
154 State *prev = nullptr;
155 for (auto s : geodesic)
156 {
157 // Make sure geodesics contain only constraint satisfying states.
158 if (flags & CONSTRAINED_STATESPACE_GEODESIC_SATISFY)
159 satisfyGeodesics |= !constraint_->isSatisfied(s);
160
161 // Make sure geodesics have some continuity.
162 if (flags & CONSTRAINED_STATESPACE_GEODESIC_CONTINUITY && prev != nullptr)
163 continuityGeodesics |= distance(prev, s) > lambda_ * delta_;
164
165 prev = s;
166 }
167
168 for (auto s : geodesic)
169 freeState(s);
170 }
171
172 if (satisfyGeodesics)
173 throw Exception("Discrete geodesic computation generates invalid states.");
174
175 if (continuityGeodesics)
176 throw Exception("Discrete geodesic computation generates non-continuous states.");
177 }
178
179 freeState(s1);
180 freeState(s2);
181
182 if (!isTraversable)
183 throw Exception("Unable to compute discrete geodesic on constraint.");
184}
185
187{
188 constrainedSanityChecks(~0);
189
190 double zero = std::numeric_limits<double>::epsilon();
191 double eps = std::numeric_limits<double>::epsilon();
192 unsigned int flags = STATESPACE_DISTANCE_DIFFERENT_STATES | STATESPACE_DISTANCE_SYMMETRIC |
193 STATESPACE_DISTANCE_BOUND | STATESPACE_RESPECT_BOUNDS | STATESPACE_ENFORCE_BOUNDS_NO_OP;
194
195 StateSpace::sanityChecks(zero, eps, flags);
196}
197
199{
200 // Check that the object is valid
201 if (si == nullptr)
202 throw ompl::Exception("ompl::base::ConstrainedStateSpace::setSpaceInformation(): "
203 "si is nullptr.");
204 if (si->getStateSpace().get() != this)
205 throw ompl::Exception("ompl::base::ConstrainedStateSpace::setSpaceInformation(): "
206 "si for ConstrainedStateSpace must be constructed from the same state space object.");
207
208 si_ = si;
209}
210
212{
213 if (delta <= 0)
214 throw ompl::Exception("ompl::base::ConstrainedStateSpace::setDelta(): "
215 "delta must be positive.");
216 delta_ = delta;
217
218 if (setup_)
219 {
220 setLongestValidSegmentFraction(delta_ / getMaximumExtent());
221 si_->setStateValidityCheckingResolution(delta_);
222 }
223}
224
226{
227 if (setup_)
228 return;
229
230 if (si_ == nullptr)
231 throw ompl::Exception("ompl::base::ConstrainedStateSpace::setup(): "
232 "Must associate a SpaceInformation object to the ConstrainedStateSpace via"
233 "setStateInformation() before use.");
234
236
237 setDelta(delta_); // This makes some setup-related calls
238 setup_ = true;
239
240 setDelta(delta_);
241
242 // Call again to make sure information propagates properly to both wrapper
243 // and underlying space.
245
246 // Check if stride length of underlying state variables is 1
247 auto *state = space_->allocState();
248 bool flag = true;
249 for (unsigned int i = 1; i < space_->getDimension() && flag; ++i)
250 {
251 std::size_t newStride = space_->getValueAddressAtIndex(state, i) - space_->getValueAddressAtIndex(state, i - 1);
252 flag = newStride == 1;
253 }
254 space_->freeState(state);
255
256 if (!flag)
257 throw ompl::Exception("ompl::base::ConstrainedStateSpace::setup(): "
258 "Stride length of member variables != 1, cannot translate into dense vector.");
259}
260
262{
263}
264
266{
267 return new StateType(this);
268}
269
270void ompl::base::ConstrainedStateSpace::interpolate(const State *from, const State *to, const double t,
271 State *state) const
272{
273 // Get the list of intermediate states along the manifold.
274 std::vector<State *> geodesic;
275
276 // Default to returning `from' if traversal fails.
277 auto temp = from;
278 if (discreteGeodesic(from, to, true, &geodesic))
279 temp = geodesicInterpolate(geodesic, t);
280
281 copyState(state, temp);
282
283 for (auto s : geodesic)
284 freeState(s);
285}
286
288 const double t) const
289{
290 unsigned int n = geodesic.size();
291 auto *d = new double[n];
292
293 // Compute partial sums of distances between intermediate states.
294 d[0] = 0.;
295 for (unsigned int i = 1; i < n; ++i)
296 d[i] = d[i - 1] + distance(geodesic[i - 1], geodesic[i]);
297
298 // Find the two adjacent states that t lies between, and return the closer.
299 const double last = d[n - 1];
300 if (last <= std::numeric_limits<double>::epsilon())
301 {
302 delete[] d;
303 return geodesic[0];
304 }
305 else
306 {
307 unsigned int i = 0;
308 while (i < (n - 1) && (d[i] / last) <= t)
309 i++;
310
311 const double t1 = d[i] / last - t;
312 const double t2 = (i <= n - 2) ? d[i + 1] / last - t : 1;
313
314 delete[] d;
315 assert((t1 < t2 || std::abs(t1 - t2) < std::numeric_limits<double>::epsilon()) ? (i < geodesic.size()) : (i + 1 < geodesic.size()));
316 return (t1 < t2 || std::abs(t1 - t2) < std::numeric_limits<double>::epsilon()) ? geodesic[i] : geodesic[i + 1];
317 }
318}
The exception type for ompl.
Definition: Exception.h:47
bool checkMotion(const State *s1, const State *s2) const override
Return whether we can step from s1 to s2 along the manifold without collision.
ConstrainedMotionValidator(SpaceInformation *si)
Constructor.
A State in a ConstrainedStateSpace, represented as a dense real vector of values. For convenience and...
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
virtual State * geodesicInterpolate(const std::vector< State * > &geodesic, double t) const
Like interpolate(...), but interpolates between intermediate states already supplied in stateList fro...
void setSpaceInformation(SpaceInformation *si)
Sets the space information for this state space. Required for collision checking in manifold traversa...
virtual void clear()
Clear any allocated memory from the state space.
State * allocState() const override
Allocate a new state in this space.
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...
void constrainedSanityChecks(unsigned int flags) const
Do some sanity checks relating to discrete geodesic computation and constraint satisfaction....
ConstrainedStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint)
Construct a constrained space from an ambientSpace and a constraint.
void sanityChecks() const override
Perform both constrained and regular sanity checks.
void setDelta(double delta)
Set delta, the step size for traversing the manifold and collision checking. Default defined by ompl:...
void setup() override
Final setup for the space.
A shared pointer wrapper for ompl::base::Constraint.
Abstract definition for a class checking the validity of motions – path segments between states....
A shared pointer wrapper for ompl::base::SpaceInformation.
The base class for space information. This contains all the information about the space planning is d...
A shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::StateSpace.
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
State space wrapper that transparently passes state space operations through to the underlying space....
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...