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 
62  class ConstrainedValidStateSampler : public ValidStateSampler
63  {
64  public:
67  ConstrainedValidStateSampler(const SpaceInformation *si)
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  {
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 
154  class TangentBundleSpaceInformation : public ConstrainedSpaceInformation
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...
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:113
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.
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.
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...
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.
Main namespace. Contains everything in this library.