Loading...
Searching...
No Matches
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{
111 const ob::RealVectorStateSpace::StateType& pos = *state->as<ob::RealVectorStateSpace::StateType>();
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...
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...
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.
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()