ConstrainedStateSpace.h
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 #ifndef OMPL_BASE_SPACES_CONSTRAINED_STATE_SPACE_
38 #define OMPL_BASE_SPACES_CONSTRAINED_STATE_SPACE_
39 
40 #include "ompl/base/Constraint.h"
41 #include "ompl/base/StateSampler.h"
42 #include "ompl/base/ValidStateSampler.h"
43 #include "ompl/base/MotionValidator.h"
44 #include "ompl/base/spaces/WrapperStateSpace.h"
45 #include "ompl/base/SpaceInformation.h"
46 
47 #include <Eigen/Core>
48 
49 namespace ompl
50 {
51  namespace magic
52  {
53  static const double CONSTRAINED_STATE_SPACE_DELTA = 0.05;
54  static const double CONSTRAINED_STATE_SPACE_LAMBDA = 2.0;
55  }
56 
57  namespace base
58  {
60 
61  OMPL_CLASS_FORWARD(ConstrainedStateSpace);
63 
66  class ConstrainedMotionValidator : public MotionValidator
67  {
68  public:
70  ConstrainedMotionValidator(SpaceInformation *si);
71 
74 
77  bool checkMotion(const State *s1, const State *s2) const override;
78 
86  bool checkMotion(const State *s1, const State *s2, std::pair<State *, double> &lastValid) const override;
87 
88  protected:
90  const ConstrainedStateSpace &ss_;
91  };
92 
133  class ConstrainedStateSpace : public WrapperStateSpace
134  {
135  public:
138  enum SanityChecks
139  {
143 
147 
151 
155 
159  };
160 
168  class StateType : public WrapperStateSpace::StateType, public Eigen::Map<Eigen::VectorXd>
169  {
170  public:
172  StateType(const ConstrainedStateSpace *space)
173  : WrapperStateSpace::StateType(space->getSpace()->allocState())
174  , Eigen::Map<Eigen::VectorXd>(space->getValueAddressAtIndex(this, 0), space->getDimension())
175  {
176  }
177 
181  void copy(const Eigen::Ref<const Eigen::VectorXd> &other)
182  {
183  // Explicitly cast and call `=` on the state as an
184  // Eigen::Map<...>. This copies the other.
185  static_cast<Eigen::Map<Eigen::VectorXd> *>(this)->operator=(other);
186  }
187  };
188 
191  ConstrainedStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint);
192 
199  bool isMetricSpace() const override
200  {
201  return false;
202  }
203 
207 
209  void setup() override;
210 
212  virtual void clear();
213 
215  State *allocState() const override;
216 
219  void constrainedSanityChecks(unsigned int flags) const;
220 
222  void sanityChecks() const override;
223 
226  unsigned int validSegmentCount(const State* s1, const State* s2) const override
227  {
228  return distance(s1, s2) * (1. / delta_) * lambda_;
229  }
230 
240  void interpolate(const State *from, const State *to, double t, State *state) const override;
241 
251  virtual bool discreteGeodesic(const State *from, const State *to, bool interpolate = false,
252  std::vector<State *> *geodesic = nullptr) const = 0;
253 
259  virtual State *geodesicInterpolate(const std::vector<State *> &geodesic, double t) const;
260 
269  void setDelta(double delta);
270 
275  void setLambda(double lambda)
276  {
277  if (lambda <= 1)
278  throw ompl::Exception("ompl::base::AtlasStateSpace::setLambda(): "
279  "lambda must be > 1.");
280  lambda_ = lambda;
281  }
282 
284  double getDelta() const
285  {
286  return delta_;
287  }
288 
290  double getLambda() const
291  {
292  return lambda_;
293  }
294 
296  unsigned int getAmbientDimension() const
297  {
298  return n_;
299  }
300 
302  unsigned int getManifoldDimension() const
303  {
304  return k_;
305  }
306 
308  const ConstraintPtr getConstraint() const
309  {
310  return constraint_;
311  }
312 
315  protected:
318  SpaceInformation *si_{nullptr};
319 
322 
324  const unsigned int n_;
325 
327  const unsigned int k_;
328 
330  double delta_;
331 
336  double lambda_{ompl::magic::CONSTRAINED_STATE_SPACE_LAMBDA};
337 
339  bool setup_{false};
340  };
341  }
342 }
343 
344 #endif
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...
@ CONSTRAINED_STATESPACE_GEODESIC_INTERPOLATE
Check whether geodesicInterpolate(...) returns constraint satisfying states.
const ConstraintPtr getConstraint() const
Returns the constraint that defines the underlying manifold.
SanityChecks
Flags used in a bit mask for constrained state space sanity checks, constraintSanityChecks().
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.
SpaceInformation * si_
SpaceInformation associated with this space. Required for early collision checking in manifold traver...
Definition of an abstract state.
Definition: State.h:113
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...
virtual void clear()
Clear any allocated memory from the state space.
double * getValueAddressAtIndex(State *state, unsigned int index) const override
Many states contain a number of double values. This function provides a means to get the memory addre...
double lambda_
Manifold traversal from x to y is stopped if accumulated distance is greater than d(x,...
void copy(const Eigen::Ref< const Eigen::VectorXd > &other)
Copy the contents from a vector into this state. Uses the underlying copy operator used by Eigen for ...
bool setup_
Whether setup() has been called.
virtual bool discreteGeodesic(const State *from, const State *to, bool interpolate=false, std::vector< State * > *geodesic=nullptr) const =0
Traverse the manifold from from toward to. Returns true if we reached to, and false if we stopped ear...
double getLambda() const
Get lambda (see setLambda()).
double getDelta() const
Get delta, the step size across the manifold.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
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....
double delta_
Step size when traversing the manifold and collision checking.
const unsigned int k_
Manifold dimension.
bool isMetricSpace() const override
Returns false as the implicit constrained configuration space defined by the constraint is not metric...
const unsigned int n_
Ambient space dimension.
@ CONSTRAINED_STATESPACE_SAMPLERS
Check whether state samplers return constraint satisfying samples.
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
@ CONSTRAINED_STATESPACE_GEODESIC_CONTINUITY
Check whether discrete geodesics keep lambda_ * delta_ continuity.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
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.
const ConstrainedStateSpace & ss_
Space in which we check motion.
State * allocState() const override
Allocate a new state in this space.
unsigned int getAmbientDimension() const
Returns the dimension of the ambient space.
void setLambda(double lambda)
Set lambda, where lambda * distance(x, y) is the maximum length of the geodesic x to y....
unsigned int validSegmentCount(const State *s1, const State *s2) const override
Return the valid segment count on the manifold, as valid segment count is determined by delta_ and la...
State space wrapper that transparently passes state space operations through to the underlying space....
The exception type for ompl.
Definition: Exception.h:78
unsigned int getManifoldDimension() const
Returns the dimension of the manifold.
void sanityChecks() const override
Perform both constrained and regular sanity checks.
@ CONSTRAINED_STATESPACE_GEODESIC_SATISFY
Check whether discrete geodesics satisfy the constraint at all points.
void setSpaceInformation(SpaceInformation *si)
Sets the space information for this state space. Required for collision checking in manifold traversa...
StateType(const ConstrainedStateSpace *space)
Constructor. Requires space to setup information about underlying state.
const ConstraintPtr constraint_
Constraint function that defines the manifold.
Main namespace. Contains everything in this library.
@ CONSTRAINED_STATESPACE_JACOBIAN
Check if the constraint's numerical Jacobian approximates its provided Jacobian.