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 
52 namespace ompl
53 {
54  namespace base
55  {
57 
58  OMPL_CLASS_FORWARD(ConstrainedSpaceInformation);
60 
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 
110  {
111  public:
114  {
115  stateSpace_->as<ConstrainedStateSpace>()->setSpaceInformation(this);
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:
159  {
160  }
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
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...
The base class for space information. This contains all the information about the space planning is d...
A state in an atlas represented as a real vector in ambient space and a chart that it belongs to.
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...
A shared pointer wrapper for ompl::base::Constraint.
Definition of an abstract state.
Definition: State.h:50
ConstrainedSpaceInformation(StateSpacePtr space)
Constructor. Sets the instance of the state space to plan with.
unsigned int attempts_
Number of attempts to find a valid sample.
StateSpacePtr stateSpace_
The state space planning is to be performed in.
void freeState(State *state) const
Free the memory of a state.
const SpaceInformation * si_
The state space this sampler samples.
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.
Space information for a constrained state space. Implements more direct for getting motion states.
Valid state sampler for constrained state spaces.
TangentBundleSpaceInformation(StateSpacePtr space)
Constructor. Sets the instance of the state space to plan with.
bool sampleNear(State *state, const State *near, double distance) override
Sample a state near another, within specified distance. Return false, in case of failure.
State * cloneState(const State *source) const
Clone a state.
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
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...
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::StateSpace.
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...
A shared pointer wrapper for ompl::base::StateSampler.
ConstrainedStateSpace encapsulating a planner-agnostic lazy atlas algorithm for planning on a constra...
Space information for a tangent bundle-based state space. Implements more direct for getting motion s...
MotionValidatorPtr motionValidator_
The instance of the motion validator to use when determining the validity of motions in the planning ...
bool isValid(const State *state) const
Check if a given state is valid or not.
Abstract definition of a state sampler.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22