SyclopRRT.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2011, 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: Matt Maly */
36
37#include "ompl/control/planners/syclop/SyclopRRT.h"
38#include "ompl/base/goals/GoalSampleableRegion.h"
39#include "ompl/tools/config/SelfConfig.h"
40
42{
44 sampler_ = si_->allocStateSampler();
45 controlSampler_ = siC_->allocDirectedControlSampler();
46 lastGoalMotion_ = nullptr;
47
48 // Create a default GNAT nearest neighbors structure if the user doesn't want
49 // the default regionalNN check from the discretization
50 if (!nn_ && !regionalNN_)
51 {
52 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion *>(this));
53 nn_->setDistanceFunction([this](Motion *a, const Motion *b)
54 {
55 return distanceFunction(a, b);
56 });
57 }
58}
59
61{
63 freeMemory();
64 if (nn_)
65 nn_->clear();
66 lastGoalMotion_ = nullptr;
67}
68
70{
71 Planner::getPlannerData(data);
72 std::vector<Motion *> motions;
73 if (nn_)
74 nn_->list(motions);
75 double delta = siC_->getPropagationStepSize();
76
77 if (lastGoalMotion_ != nullptr)
78 data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
79
80 for (auto &motion : motions)
81 {
82 if (motion->parent != nullptr)
83 {
84 if (data.hasControls())
85 data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state),
86 control::PlannerDataEdgeControl(motion->control, motion->steps * delta));
87 else
88 data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
89 }
90 else
91 data.addStartVertex(base::PlannerDataVertex(motion->state));
92 }
93}
94
96{
97 auto *motion = new Motion(siC_);
98 si_->copyState(motion->state, s);
99 siC_->nullControl(motion->control);
100
101 if (nn_)
102 nn_->add(motion);
103 return motion;
104}
105
106void ompl::control::SyclopRRT::selectAndExtend(Region &region, std::vector<Motion *> &newMotions)
107{
108 auto *rmotion = new Motion(siC_);
109 base::StateSamplerPtr sampler(si_->allocStateSampler());
110 std::vector<double> coord(decomp_->getDimension());
111 decomp_->sampleFromRegion(region.index, rng_, coord);
112 decomp_->sampleFullState(sampler, coord, rmotion->state);
113
114 Motion *nmotion;
115 if (regionalNN_)
116 {
117 /* Instead of querying the nearest neighbors datastructure over the entire tree of motions,
118 * here we perform a linear search over all motions in the selected region and its neighbors. */
119 std::vector<int> searchRegions;
120 decomp_->getNeighbors(region.index, searchRegions);
121 searchRegions.push_back(region.index);
122
123 std::vector<Motion *> motions;
124 for (const auto &i : searchRegions)
125 {
126 const std::vector<Motion *> &regionMotions = getRegionFromIndex(i).motions;
127 motions.insert(motions.end(), regionMotions.begin(), regionMotions.end());
128 }
129
130 std::vector<Motion *>::const_iterator i = motions.begin();
131 nmotion = *i;
132 double minDistance = distanceFunction(rmotion, nmotion);
133 ++i;
134 while (i != motions.end())
135 {
136 Motion *m = *i;
137 const double dist = distanceFunction(rmotion, m);
138 if (dist < minDistance)
139 {
140 nmotion = m;
141 minDistance = dist;
142 }
143 ++i;
144 }
145 }
146 else
147 {
148 assert(nn_);
149 nmotion = nn_->nearest(rmotion);
150 }
151
152 unsigned int duration =
153 controlSampler_->sampleTo(rmotion->control, nmotion->control, nmotion->state, rmotion->state);
154 if (duration >= siC_->getMinControlDuration())
155 {
156 rmotion->steps = duration;
157 rmotion->parent = nmotion;
158 newMotions.push_back(rmotion);
159 if (nn_)
160 nn_->add(rmotion);
161 lastGoalMotion_ = rmotion;
162 }
163 else
164 {
165 si_->freeState(rmotion->state);
166 siC_->freeControl(rmotion->control);
167 delete rmotion;
168 }
169}
170
172{
173 if (nn_)
174 {
175 std::vector<Motion *> motions;
176 nn_->list(motions);
177 for (auto m : motions)
178 {
179 if (m->state != nullptr)
180 si_->freeState(m->state);
181 if (m->control != nullptr)
182 siC_->freeControl(m->control);
183 delete m;
184 }
185 }
186}
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance.
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
Definition of an abstract state.
Definition: State.h:50
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:61
DirectedControlSamplerPtr allocDirectedControlSampler() const
Allocate an instance of the DirectedControlSampler to use. This will be the default (SimpleDirectedCo...
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: SyclopRRT.cpp:69
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: SyclopRRT.h:116
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SyclopRRT.cpp:60
void selectAndExtend(Region &region, std::vector< Motion * > &newMotions) override
Select a Motion from the given Region, and extend the tree from the Motion. Add any new motions creat...
Definition: SyclopRRT.cpp:106
Syclop::Motion * addRoot(const base::State *s) override
Add State s as a new root in the low-level tree, and return the Motion corresponding to s.
Definition: SyclopRRT.cpp:95
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: SyclopRRT.h:105
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SyclopRRT.cpp:41
void freeMemory()
Free the memory allocated by this planner.
Definition: SyclopRRT.cpp:171
Representation of a motion.
Definition: Syclop.h:257
base::State * state
The state contained by the motion.
Definition: Syclop.h:268
Control * control
The control contained by the motion.
Definition: Syclop.h:270
Representation of a region in the Decomposition assigned to Syclop.
Definition: Syclop.h:279
int index
The index of the graph node corresponding to this region.
Definition: Syclop.h:312
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Syclop.cpp:49
const SpaceInformation * siC_
Handle to the control::SpaceInformation object.
Definition: Syclop.h:384
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Syclop.cpp:64