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 <eigen3/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 
67  {
68  public:
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:
91  };
92 
132  {
133  public:
137  {
141 
145 
149 
153 
157  };
158 
166  class StateType : public WrapperStateSpace::StateType, public Eigen::Map<Eigen::VectorXd>
167  {
168  public:
171  : WrapperStateSpace::StateType(space->getSpace()->allocState())
172  , Eigen::Map<Eigen::VectorXd>(space->getValueAddressAtIndex(this, 0), space->getDimension())
173  {
174  }
175 
179  void copy(const Eigen::Ref<const Eigen::VectorXd> &other)
180  {
181  // Explicitly cast and call `=` on the state as an
182  // Eigen::Map<...>. This copies the other.
183  static_cast<Eigen::Map<Eigen::VectorXd> *>(this)->operator=(other);
184  }
185  };
186 
189  ConstrainedStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint);
190 
197  bool isMetricSpace() const override
198  {
199  return false;
200  }
201 
205 
207  void setup() override;
208 
210  virtual void clear();
211 
213  State *allocState() const override;
214 
217  void constrainedSanityChecks(unsigned int flags) const;
218 
220  void sanityChecks() const override;
221 
224  virtual unsigned int validSegmentCount(const State* s1, const State* s2) const override
225  {
226  return distance(s1, s2) * (1. / delta_) * lambda_;
227  }
228 
238  void interpolate(const State *from, const State *to, double t, State *state) const override;
239 
249  virtual bool discreteGeodesic(const State *from, const State *to, bool interpolate = false,
250  std::vector<State *> *geodesic = nullptr) const = 0;
251 
257  virtual State *geodesicInterpolate(const std::vector<State *> &geodesic, double t) const;
258 
267  void setDelta(const double delta);
268 
273  void setLambda(double lambda)
274  {
275  if (lambda <= 1)
276  throw ompl::Exception("ompl::base::AtlasStateSpace::setLambda(): "
277  "lambda must be > 1.");
278  lambda_ = lambda;
279  }
280 
282  double getDelta() const
283  {
284  return delta_;
285  }
286 
288  double getLambda() const
289  {
290  return lambda_;
291  }
292 
294  unsigned int getAmbientDimension() const
295  {
296  return n_;
297  }
298 
300  unsigned int getManifoldDimension() const
301  {
302  return k_;
303  }
304 
307  {
308  return constraint_;
309  }
310 
313  protected:
317 
320 
322  const unsigned int n_;
323 
325  const unsigned int k_;
326 
328  double delta_;
329 
334  double lambda_{ompl::magic::CONSTRAINED_STATE_SPACE_LAMBDA};
335 
337  bool setup_{false};
338  };
339  }
340 }
341 
342 #endif
const unsigned int k_
Manifold dimension.
const unsigned int n_
Ambient space dimension.
double delta_
Step size when traversing the manifold and collision checking.
A shared pointer wrapper for ompl::base::StateSpace.
void setup() override
Final setup for the 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 getDelta() const
Get delta, the step size across the manifold.
const ConstrainedStateSpace & ss_
Space in which we check motion.
A shared pointer wrapper for ompl::base::Constraint.
Check whether state samplers return constraint satisfying samples.
StateType(const ConstrainedStateSpace *space)
Constructor. Requires space to setup information about underlying state.
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...
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:...
Check if the constraint&#39;s numerical Jacobian approximates its provided Jacobian.
virtual 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...
unsigned int getManifoldDimension() const
Returns the dimension of the manifold.
ConstrainedStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint)
Construct a constrained space from an ambientSpace and a constraint.
const ConstraintPtr constraint_
Constraint function that defines the manifold.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
SpaceInformation * si_
SpaceInformation associated with this space. Required for early collision checking in manifold traver...
Check whether geodesicInterpolate(...) returns constraint satisfying states.
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.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
bool setup_
Whether setup() has been called.
bool isMetricSpace() const override
Returns false as the implicit constrained configuration space defined by the constraint is not metric...
void setLambda(double lambda)
Set lambda, where lambda * distance(x, y) is the maximum length of the geodesic x to y...
SanityChecks
Flags used in a bit mask for constrained state space sanity checks, constraintSanityChecks().
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...
Check whether discrete geodesics keep lambda_ * delta_ continuity.
Check whether discrete geodesics satisfy the constraint at all points.
unsigned int getAmbientDimension() const
Returns the dimension of the ambient space.
A shared pointer wrapper for ompl::base::SpaceInformation.
double lambda_
Manifold traversal from x to y is stopped if accumulated distance is greater than d(x...
The base class for space information. This contains all the information about the space planning is d...
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
double getLambda() const
Get lambda (see setLambda()).
ConstrainedMotionValidator(SpaceInformation *si)
Constructor.
The exception type for ompl.
Definition: Exception.h:46
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...
Wrapper state type. Contains a reference to an underlying state.
virtual void clear()
Clear any allocated memory from the state space.
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 ...
virtual State * geodesicInterpolate(const std::vector< State *> &geodesic, double t) const
Like interpolate(...), but interpolates between intermediate states already supplied in stateList fro...
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...
const ConstraintPtr getConstraint() const
Returns the constraint that defines the underlying manifold.
Constrained configuration space specific implementation of checkMotion() that uses discreteGeodesic()...
void sanityChecks() const override
Perform both constrained and regular sanity checks.