Loading...
Searching...
No Matches
ConstrainedSpaceInformation.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_CONSTRAINED_SPACE_INFORMATION_
38#define OMPL_BASE_CONSTRAINED_SPACE_INFORMATION_
39
40#include <utility>
41
42#include "ompl/base/SpaceInformation.h"
43#include "ompl/base/spaces/constraint/ConstrainedStateSpace.h"
44#include "ompl/base/spaces/constraint/AtlasChart.h"
45#include "ompl/base/spaces/constraint/AtlasStateSpace.h"
46#include "ompl/base/spaces/constraint/TangentBundleStateSpace.h"
47
48#include "ompl/util/ClassForward.h"
49#include "ompl/util/Console.h"
50#include "ompl/util/Exception.h"
51
52namespace ompl
53{
54 namespace base
55 {
57
58 OMPL_CLASS_FORWARD(ConstrainedSpaceInformation);
60
62 class ConstrainedValidStateSampler : public ValidStateSampler
63 {
64 public:
68 : ValidStateSampler(si)
69 , sampler_(si->getStateSpace()->allocStateSampler())
70 , constraint_(si->getStateSpace()->as<ompl::base::ConstrainedStateSpace>()->getConstraint())
71 {
72 }
73
74 bool sample(State *state) override
75 {
76 // Rejection sample for at most attempts_ tries.
77 unsigned int tries = 0;
78 bool valid;
79
80 do
81 sampler_->sampleUniform(state);
82 while (!(valid = si_->isValid(state) && constraint_->isSatisfied(state)) && ++tries < attempts_);
83
84 return valid;
85 }
86
87 bool sampleNear(State *state, const State *near, double distance) override
88 {
89 // Rejection sample for at most attempts_ tries.
90 unsigned int tries = 0;
91 bool valid;
92 do
93 sampler_->sampleUniformNear(state, near, distance);
94 while (!(valid = si_->isValid(state) && constraint_->isSatisfied(state)) && ++tries < attempts_);
95
96 return valid;
97 }
98
99 private:
101 StateSamplerPtr sampler_;
102
104 const ConstraintPtr constraint_;
105 };
106
109 class ConstrainedSpaceInformation : public SpaceInformation
110 {
111 public:
113 ConstrainedSpaceInformation(StateSpacePtr space) : SpaceInformation(std::move(space))
114 {
116 setValidStateSamplerAllocator([](const SpaceInformation *si) -> std::shared_ptr<ValidStateSampler> {
117 return std::make_shared<ConstrainedValidStateSampler>(si);
118 });
119 }
120
133 unsigned int getMotionStates(const State *s1, const State *s2, std::vector<State *> &states,
134 unsigned int /*count*/, bool endpoints, bool /*alloc*/) const override
135 {
136 bool success = stateSpace_->as<ConstrainedStateSpace>()->discreteGeodesic(s1, s2, true, &states);
137
138 if (endpoints)
139 {
140 if (!success && states.empty())
141 states.push_back(cloneState(s1));
142
143 if (success)
144 states.push_back(cloneState(s2));
145 }
146
147 return states.size();
148 }
149 };
150
155 {
156 public:
161
177 unsigned int getMotionStates(const State *s1, const State *s2, std::vector<State *> &states,
178 unsigned int /*count*/, bool /*endpoints*/, bool /*alloc*/) const override
179 {
180 auto &&atlas = stateSpace_->as<TangentBundleStateSpace>();
181
182 std::vector<State *> temp;
183 bool success = atlas->discreteGeodesic(s1, s2, true, &temp);
184
185 if (!success && temp.empty())
186 temp.push_back(cloneState(s1));
187
188 auto it = temp.begin();
189 for (; it != temp.end(); ++it)
190 {
191 auto astate = (*it)->as<AtlasStateSpace::StateType>();
192 if (!atlas->project(astate))
193 break;
194
195 states.push_back(astate);
196 }
197
198 while (it != temp.end())
199 freeState(*it++);
200
201 return states.size();
202 }
203
216 bool checkMotion(const State *s1, const State *s2, std::pair<State *, double> &lastValid) const override
217 {
218 auto &&atlas = stateSpace_->as<TangentBundleStateSpace>();
219 bool valid = motionValidator_->checkMotion(s1, s2, lastValid);
220
221 if (lastValid.first != nullptr)
222 {
223 auto astate = lastValid.first->as<AtlasStateSpace::StateType>();
224 if (!atlas->project(astate))
225 valid = false;
226 }
227
228 return valid;
229 }
230 };
231 }
232}
233
234#endif
A state in an atlas represented as a real vector in ambient space and a chart that it belongs to.
Space information for a constrained state space. Implements more direct for getting motion states.
unsigned int getMotionStates(const State *s1, const State *s2, std::vector< State * > &states, unsigned int, bool endpoints, bool) const override
Get count states that make up a motion between s1 and s2. Returns the number of states that were adde...
ConstrainedSpaceInformation(StateSpacePtr space)
Constructor. Sets the instance of the state space to plan with.
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
void setSpaceInformation(SpaceInformation *si)
Sets the space information for this state space. Required for collision checking in manifold traversa...
bool sampleNear(State *state, const State *near, double distance) override
Sample a state near another, within specified distance. Return false, in case of failure.
ConstrainedValidStateSampler(const SpaceInformation *si)
Constructor. Create a valid state sampler for a constrained state space.
bool sample(State *state) override
Sample a state. Return false in case of failure.
A shared pointer wrapper for ompl::base::Constraint.
The base class for space information. This contains all the information about the space planning is d...
void freeState(State *state) const
Free the memory of a state.
StateSpacePtr stateSpace_
The state space planning is to be performed in.
MotionValidatorPtr motionValidator_
The instance of the motion validator to use when determining the validity of motions in the planning ...
State * cloneState(const State *source) const
Clone a state.
void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa)
Set the allocator to use for a valid state sampler. This replaces the default uniform valid state sam...
A shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::StateSpace.
Definition of an abstract state.
Definition State.h:50
unsigned int getMotionStates(const State *s1, const State *s2, std::vector< State * > &states, unsigned int, bool, bool) const override
Get count states that make up a motion between s1 and s2. Returns the number of states that were adde...
TangentBundleSpaceInformation(StateSpacePtr space)
Constructor. Sets the instance of the state space to plan with.
bool checkMotion(const State *s1, const State *s2, std::pair< State *, double > &lastValid) const override
Incrementally check if the path between two motions is valid. Also compute the last state that was va...
ConstrainedStateSpace encapsulating a planner-agnostic lazy atlas algorithm for planning on a constra...
bool discreteGeodesic(const State *from, const State *to, bool interpolate=false, std::vector< State * > *geodesic=nullptr) const override
Traverse the manifold from from toward to. Returns true if we reached to, and false if we stopped ear...
const SpaceInformation * si_
The state space this sampler samples.
unsigned int attempts_
Number of attempts to find a valid sample.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Main namespace. Contains everything in this library.
STL namespace.