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
47namespace ob = ompl::base;
48namespace 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.
59class MyValidStateSampler : public ob::ValidStateSampler
60{
61public:
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; break;
81 case 1: val[0]=x-.8; val[1]=y+.8; break;
82 case 2: val[0]=y-1; val[1]=x-1; break;
83 case 3: val[0]=y+.8; val[1]=x-.8; break;
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 }
101protected:
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
109bool 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
123ob::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
131ob::ValidStateSamplerPtr allocMyValidStateSampler(const ob::SpaceInformation *si)
132{
133 return std::make_shared<MyValidStateSampler>(si);
134}
135
136
137void 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
190int 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}
The exception type for ompl.
Definition: Exception.h:47
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
The lower and upper bounds for an Rn space.
Definition of a scoped state.
Definition: ScopedState.h:57
The base class for space information. This contains all the information about the space planning is d...
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
Abstract definition of a state sampler.
virtual bool sample(State *state)=0
Sample a state. Return false in case of failure.
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.
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:48
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:64
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49