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 <boost/thread.hpp>
45 #include <iostream>
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  virtual bool sample(ob::State *state)
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  virtual bool sampleNear(ob::State*, const ob::State*, const double)
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  boost::this_thread::sleep(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.
128 }
129 
130 // return an instance of my sampler
131 ob::ValidStateSamplerPtr allocMyValidStateSampler(const ob::SpaceInformation *si)
132 {
133  return ob::ValidStateSamplerPtr(new MyValidStateSampler(si));
134 }
135 
136 
137 void plan(int samplerIndex)
138 {
139  // construct the state space we are planning in
141 
142  // set the bounds
143  ob::RealVectorBounds bounds(3);
144  bounds.setLow(-1);
145  bounds.setHigh(1);
146  space->as<ob::RealVectorStateSpace>()->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(boost::bind(&isStateValid, _1));
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  ob::PlannerPtr planner(new 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, char **)
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 boost shared pointer wrapper for ompl::base::ValidStateSampler.
Definition of a scoped state.
Definition: ScopedState.h:56
A boost shared pointer wrapper for ompl::base::StateSpace.
State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
Generate valid samples using obstacle based sampling. First sample an invalid state, then sample a valid state. Then, interpolate from the invalid state to the valid state, returning the first valid state encountered.
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:65
A boost shared pointer wrapper for ompl::base::Planner.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
Abstract definition of a state sampler.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A state space representing Rn. The distance function is the L2 norm.
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:50
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:47
virtual bool sample(State *state)=0
Sample a state. Return false in case of failure.
The lower and upper bounds for an Rn space.
Probabilistic RoadMap planner.
Definition: PRM.h:83
virtual bool sampleNear(State *state, const State *near, const double distance)=0
Sample a state near another, within specified distance. Return false, in case of failure.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:48