StateSampling.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, 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: Mark Moll */
36 
37 #include <ompl/base/SpaceInformation.h>
38 #include <ompl/base/spaces/SE3StateSpace.h>
39 #include <ompl/base/samplers/ObstacleBasedValidStateSampler.h>
40 #include <ompl/geometric/planners/prm/PRM.h>
41 #include <ompl/geometric/SimpleSetup.h>
42 
43 #include <ompl/config.h>
44 #include <iostream>
45 #include <thread>
46 
47 namespace ob = ompl::base;
48 namespace og = ompl::geometric;
49 
51 
52 
53 // This is a problem-specific sampler that automatically generates valid
54 // states; it doesn't need to call SpaceInformation::isValid. This is an
55 // example of constrained sampling. If you can explicitly describe the set valid
56 // states and can draw samples from it, then this is typically much more
57 // efficient than generating random samples from the entire state space and
58 // checking for validity.
59 class MyValidStateSampler : public ob::ValidStateSampler
60 {
61 public:
62  MyValidStateSampler(const ob::SpaceInformation *si) : ValidStateSampler(si)
63  {
64  name_ = "my sampler";
65  }
66  // Generate a sample in the valid part of the R^3 state space
67  // Valid states satisfy the following constraints:
68  // -1<= x,y,z <=1
69  // if .25 <= z <= .5, then |x|>.8 and |y|>.8
70  bool sample(ob::State *state) override
71  {
72  double* val = static_cast<ob::RealVectorStateSpace::StateType*>(state)->values;
73  double z = rng_.uniformReal(-1,1);
74 
75  if (z>.25 && z<.5)
76  {
77  double x = rng_.uniformReal(0,1.8), y = rng_.uniformReal(0,.2);
78  switch(rng_.uniformInt(0,3))
79  {
80  case 0: val[0]=x-1; val[1]=y-1;
81  case 1: val[0]=x-.8; val[1]=y+.8;
82  case 2: val[0]=y-1; val[1]=x-1;
83  case 3: val[0]=y+.8; val[1]=x-.8;
84  }
85  }
86  else
87  {
88  val[0] = rng_.uniformReal(-1,1);
89  val[1] = rng_.uniformReal(-1,1);
90  }
91  val[2] = z;
92  assert(si_->isValid(state));
93  return true;
94  }
95  // We don't need this in the example below.
96  bool sampleNear(ob::State* /*state*/, const ob::State* /*near*/, const double /*distance*/) override
97  {
98  throw ompl::Exception("MyValidStateSampler::sampleNear", "not implemented");
99  return false;
100  }
101 protected:
102  ompl::RNG rng_;
103 };
104 
106 
107 // this function is needed, even when we can write a sampler like the one
108 // above, because we need to check path segments for validity
109 bool isStateValid(const ob::State *state)
110 {
112  // Let's pretend that the validity check is computationally relatively
113  // expensive to emphasize the benefit of explicitly generating valid
114  // samples
115  std::this_thread::sleep_for(ompl::time::seconds(.0005));
116  // Valid states satisfy the following constraints:
117  // -1<= x,y,z <=1
118  // if .25 <= z <= .5, then |x|>.8 and |y|>.8
119  return !(fabs(pos[0])<.8 && fabs(pos[1])<.8 && pos[2]>.25 && pos[2]<.5);
120 }
121 
122 // return an obstacle-based sampler
123 ob::ValidStateSamplerPtr allocOBValidStateSampler(const ob::SpaceInformation *si)
124 {
125  // we can perform any additional setup / configuration of a sampler here,
126  // but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.
127  return std::make_shared<ob::ObstacleBasedValidStateSampler>(si);
128 }
129 
130 // return an instance of my sampler
131 ob::ValidStateSamplerPtr allocMyValidStateSampler(const ob::SpaceInformation *si)
132 {
133  return std::make_shared<MyValidStateSampler>(si);
134 }
135 
136 
137 void plan(int samplerIndex)
138 {
139  // construct the state space we are planning in
140  auto space(std::make_shared<ob::RealVectorStateSpace>(3));
141 
142  // set the bounds
143  ob::RealVectorBounds bounds(3);
144  bounds.setLow(-1);
145  bounds.setHigh(1);
146  space->setBounds(bounds);
147 
148  // define a simple setup class
149  og::SimpleSetup ss(space);
150 
151  // set state validity checking for this space
152  ss.setStateValidityChecker(isStateValid);
153 
154  // create a start state
155  ob::ScopedState<> start(space);
156  start[0] = start[1] = start[2] = 0;
157 
158  // create a goal state
159  ob::ScopedState<> goal(space);
160  goal[0] = goal[1] = 0.;
161  goal[2] = 1;
162 
163  // set the start and goal states
164  ss.setStartAndGoalStates(start, goal);
165 
166  // set sampler (optional; the default is uniform sampling)
167  if (samplerIndex==1)
168  // use obstacle-based sampling
169  ss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);
170  else if (samplerIndex==2)
171  // use my sampler
172  ss.getSpaceInformation()->setValidStateSamplerAllocator(allocMyValidStateSampler);
173 
174  // create a planner for the defined space
175  auto planner(std::make_shared<og::PRM>(ss.getSpaceInformation()));
176  ss.setPlanner(planner);
177 
178  // attempt to solve the problem within ten seconds of planning time
179  ob::PlannerStatus solved = ss.solve(10.0);
180  if (solved)
181  {
182  std::cout << "Found solution:" << std::endl;
183  // print the path to screen
184  ss.getSolutionPath().print(std::cout);
185  }
186  else
187  std::cout << "No solution found" << std::endl;
188 }
189 
190 int main(int /*argc*/, char ** /*argv*/)
191 {
192  std::cout << "Using default uniform sampler:" << std::endl;
193  plan(0);
194  std::cout << "\nUsing obstacle-based sampler:" << std::endl;
195  plan(1);
196  std::cout << "\nUsing my sampler:" << std::endl;
197  plan(2);
198 
199  return 0;
200 }
A shared pointer wrapper for ompl::base::ValidStateSampler.
Definition of a scoped state.
Definition: ScopedState.h:56
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:76
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
Abstract definition of a state sampler.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
The base class for space information. This contains all the information about the space planning is d...
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
The exception type for ompl.
Definition: Exception.h:46
virtual bool sample(State *state)=0
Sample a state. Return false in case of failure.
The lower and upper bounds for an Rn space.
virtual bool sampleNear(State *state, const State *near, double distance)=0
Sample a state near another, within specified distance. Return false, in case of failure.
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:47