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 
270 void 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 }
void setup() override
Final setup for the space.
The base class for space information. This contains all the information about the space planning is d...
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
A shared pointer wrapper for ompl::base::SpaceInformation.
A shared pointer wrapper for ompl::base::Constraint.
ConstrainedMotionValidator(SpaceInformation *si)
Constructor.
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...
ConstrainedStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint)
Construct a constrained space from an ambientSpace and a constraint.
Definition of an abstract state.
Definition: State.h:113
virtual void sanityChecks() const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
Definition: StateSpace.cpp:603
virtual void clear()
Clear any allocated memory from the state space.
Abstract definition for a class checking the validity of motions – path segments between states....
void setDelta(double delta)
Set delta, the step size for traversing the manifold and collision checking. Default defined by ompl:...
void constrainedSanityChecks(unsigned int flags) const
Do some sanity checks relating to discrete geodesic computation and constraint satisfaction....
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
bool checkMotion(const State *s1, const State *s2) const override
Return whether we can step from s1 to s2 along the manifold without collision.
virtual State * geodesicInterpolate(const std::vector< State * > &geodesic, double t) const
Like interpolate(...), but interpolates between intermediate states already supplied in stateList fro...
A State in a ConstrainedStateSpace, represented as a dense real vector of values. For convenience and...
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::base::StateSampler.
State * allocState() const override
Allocate a new state in this space.
State space wrapper that transparently passes state space operations through to the underlying space....
The exception type for ompl.
Definition: Exception.h:78
void sanityChecks() const override
Perform both constrained and regular sanity checks.
void setSpaceInformation(SpaceInformation *si)
Sets the space information for this state space. Required for collision checking in manifold traversa...