Loading...
Searching...
No Matches
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
49namespace 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
134 {
135 public:
160
168 class StateType : public WrapperStateSpace::StateType, public Eigen::Map<Eigen::VectorXd>
169 {
170 public:
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
309 {
310 return constraint_;
311 }
312
315 protected:
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
The exception type for ompl.
Definition Exception.h:47
Constrained configuration space specific implementation of checkMotion() that uses discreteGeodesic()...
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.
const ConstrainedStateSpace & ss_
Space in which we check motion.
A State in a ConstrainedStateSpace, represented as a dense real vector of values. For convenience and...
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 ...
StateType(const ConstrainedStateSpace *space)
Constructor. Requires space to setup information about underlying state.
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 setLambda(double lambda)
Set lambda, where lambda * distance(x, y) is the maximum length of the geodesic x to y....
double getDelta() const
Get delta, the step size across the manifold.
double getLambda() const
Get lambda (see setLambda()).
void setSpaceInformation(SpaceInformation *si)
Sets the space information for this state space. Required for collision checking in manifold traversa...
const ConstraintPtr getConstraint() const
Returns the constraint that defines the underlying manifold.
virtual void clear()
Clear any allocated memory from the state space.
bool setup_
Whether setup() has been called.
double lambda_
Manifold traversal from x to y is stopped if accumulated distance is greater than d(x,...
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...
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 delta_
Step size when traversing the manifold and collision checking.
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.
unsigned int getAmbientDimension() const
Returns the dimension of the ambient space.
const unsigned int n_
Ambient space dimension.
bool isMetricSpace() const override
Returns false as the implicit constrained configuration space defined by the constraint is not metric...
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...
SanityChecks
Flags used in a bit mask for constrained state space sanity checks, constraintSanityChecks().
@ CONSTRAINED_STATESPACE_GEODESIC_SATISFY
Check whether discrete geodesics satisfy the constraint at all points.
@ CONSTRAINED_STATESPACE_SAMPLERS
Check whether state samplers return constraint satisfying samples.
@ CONSTRAINED_STATESPACE_GEODESIC_CONTINUITY
Check whether discrete geodesics keep lambda_ * delta_ continuity.
@ CONSTRAINED_STATESPACE_JACOBIAN
Check if the constraint's numerical Jacobian approximates its provided Jacobian.
@ CONSTRAINED_STATESPACE_GEODESIC_INTERPOLATE
Check whether geodesicInterpolate(...) returns constraint satisfying states.
void sanityChecks() const override
Perform both constrained and regular sanity checks.
const unsigned int k_
Manifold dimension.
void setDelta(double delta)
Set delta, the step size for traversing the manifold and collision checking. Default defined by ompl:...
SpaceInformation * si_
SpaceInformation associated with this space. Required for early collision checking in manifold traver...
void setup() override
Final setup for the space.
const ConstraintPtr constraint_
Constraint function that defines the manifold.
unsigned int getManifoldDimension() const
Returns the dimension of the manifold.
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::StateSpace.
Definition of an abstract state.
Definition State.h:50
Wrapper state type. Contains a reference to an underlying state.
State space wrapper that transparently passes state space operations through to the underlying 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...
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
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...
Main namespace. Contains everything in this library.