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_(std::move(constraint))
105  , n_(space->getDimension())
106  , k_(constraint_->getManifoldDimension())
107 {
108  setDelta(magic::CONSTRAINED_STATE_SPACE_DELTA);
109 }
110 
112 {
113  StateType *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  double *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  return (t1 < t2) ? geodesic[i] : geodesic[i + 1];
316  }
317 }
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::base::StateSampler.
void setup() override
Final setup for the space.
A shared pointer wrapper for ompl::base::Constraint.
STL namespace.
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...
A State in a ConstrainedStateSpace, represented as a dense real vector of values. For convenience and...
void setDelta(const double delta)
Set delta, the step size for traversing the manifold and collision checking. Default defined by ompl:...
ConstrainedStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint)
Construct a constrained space from an ambientSpace and a constraint.
Abstract definition for a class checking the validity of motions – path segments between states...
bool checkMotion(const State *s1, const State *s2) const override
Return whether we can step from s1 to s2 along the manifold without collision.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
State space wrapper that transparently passes state space operations through to the underlying space...
void setSpaceInformation(SpaceInformation *si)
Sets the space information for this state space. Required for collision checking in manifold traversa...
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...
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them and call their setup() functions. It is safe to call this function multiple times. At a subsequent call, projections that have been previously user configured are not re-instantiated, but their setup() method is still called.
void constrainedSanityChecks(unsigned int flags) const
Do some sanity checks relating to discrete geodesic computation and constraint satisfaction. See SanityChecks flags.
Definition of an abstract state.
Definition: State.h:49
ConstrainedMotionValidator(SpaceInformation *si)
Constructor.
The exception type for ompl.
Definition: Exception.h:46
virtual void sanityChecks() const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
State * allocState() const override
Allocate a new state in this space.
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
virtual void clear()
Clear any allocated memory from the state space.
virtual State * geodesicInterpolate(const std::vector< State *> &geodesic, double t) const
Like interpolate(...), but interpolates between intermediate states already supplied in stateList fro...
void sanityChecks() const override
Perform both constrained and regular sanity checks.