Loading...
Searching...
No Matches
ConditionalStateSampler.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2021, Technische Universität Berlin (TU Berlin)
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 TU Berlin 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: Francesco Grothe */
36
37#include "ompl/base/samplers/ConditionalStateSampler.h"
38
40 ompl::base::Motion *&startMotion,
41 std::vector<Motion *> &goalMotions,
42 std::vector<Motion *> &newBatchGoalMotions,
43 bool &sampleOldBatch)
45 , startMotion_(startMotion)
46 , goalMotions_(goalMotions)
47 , newBatchGoalMotions_(newBatchGoalMotions)
48 , sampleOldBatch_(sampleOldBatch)
49{
50 name_ = "ConditionalSampler";
51}
52
54{
55 for (int i = 0; i < maxTries_; ++i)
56 {
57 internalSampler_->sampleUniform(state);
58 double leftBound, rightBound;
59 // get minimum time, when the state can be reached from the start
60 double startBound = startMotion_->state->as<base::CompoundState>()
61 ->as<base::TimeStateSpace::StateType>(1)
62 ->position +
63 si_->getStateSpace()->as<base::SpaceTimeStateSpace>()->timeToCoverDistance(
64 state, startMotion_->state);
65 // sample old batch
66 if (sampleOldBatch_)
67 {
68 leftBound = startBound;
69 // get maximum time, at which any goal can be reached from the state
70 rightBound = std::numeric_limits<double>::min();
71 for (auto goal : goalMotions_)
72 {
73 double t = goal->state->as<base::CompoundState>()
74 ->as<base::TimeStateSpace::StateType>(1)
75 ->position -
76 si_->getStateSpace()->as<base::SpaceTimeStateSpace>()->timeToCoverDistance(
77 goal->state, state);
78 if (t > rightBound)
79 {
80 rightBound = t;
81 }
82 }
83 }
84 // sample new batch
85 else
86 {
87 // get maximum time, at which any goal from the new batch can be reached from the state
88 rightBound = std::numeric_limits<double>::min();
89 for (auto goal : newBatchGoalMotions_)
90 {
91 double t = goal->state->as<base::CompoundState>()
92 ->as<base::TimeStateSpace::StateType>(1)
93 ->position -
94 si_->getStateSpace()->as<base::SpaceTimeStateSpace>()->timeToCoverDistance(
95 goal->state, state);
96 if (t > rightBound)
97 {
98 rightBound = t;
99 }
100 }
101 // get maximum time, at which any goal from the old batch can be reached from the state
102 // only allow the left bound to be smaller than the right bound
103 leftBound = std::numeric_limits<double>::min();
104 for (auto goal : goalMotions_)
105 {
106 double t = goal->state->as<base::CompoundState>()
107 ->as<base::TimeStateSpace::StateType>(1)
108 ->position -
109 si_->getStateSpace()->as<base::SpaceTimeStateSpace>()->timeToCoverDistance(
110 goal->state, state);
111 if (t > leftBound && t < rightBound)
112 {
113 leftBound = t;
114 }
115 }
116 leftBound = std::max(leftBound, startBound);
117 }
118
119 if (leftBound <= rightBound)
120 {
121 double time = rng_.uniformReal(leftBound, rightBound);
122 state->as<base::CompoundState>()->as<base::TimeStateSpace::StateType>(1)->position = time;
123 return true;
124 }
125 }
126 return false;
127}
128
130{
131 throw ompl::Exception("ConditionalSampler::sampleNear", "not implemented");
132}
The exception type for ompl.
Definition Exception.h:47
Definition of a compound state.
Definition State.h:87
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition State.h:95
bool sampleNear(State *state, const State *near, double distance) override
Sample a state near another, within specified distance. Return false, in case of failure.
bool sample(State *state) override
Sample a state. Return false in case of failure.
ConditionalStateSampler(const SpaceInformation *si, Motion *&startMotion, std::vector< Motion * > &goalMotions, std::vector< Motion * > &newBatchGoalMotions, bool &sampleOldBatch)
The constructor.
Representation of a motion.
The base class for space information. This contains all the information about the space planning is d...
A state space consisting of a space and a time component.
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.
std::string name_
The name of the sampler.