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 {
43  Syclop::setup();
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 {
62  Syclop::clear();
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 
106 void 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 }
DirectedControlSamplerPtr allocDirectedControlSampler() const
Allocate an instance of the DirectedControlSampler to use. This will be the default (SimpleDirectedCo...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:174
base::State * state
The state contained by the motion.
Definition: Syclop.h:269
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SyclopRRT.cpp:41
int index
The index of the graph node corresponding to this region.
Definition: Syclop.h:317
A shared pointer wrapper for ompl::base::StateSampler.
Representation of a region in the Decomposition assigned to Syclop.
Definition: Syclop.h:281
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...
const SpaceInformation * siC_
Handle to the control::SpaceInformation object.
Definition: Syclop.h:392
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:60
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: SyclopRRT.h:116
Representation of a motion.
Definition: Syclop.h:257
Control * control
The control contained by the motion.
Definition: Syclop.h:271
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
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Syclop.cpp:48
void freeMemory()
Free the memory allocated by this planner.
Definition: SyclopRRT.cpp:171
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
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
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SyclopRRT.cpp:60
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...
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...
Definition of an abstract state.
Definition: State.h:49
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: SyclopRRT.h:105
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Syclop.cpp:63
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:406