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 }
base::State * state
The state contained by the motion.
Definition: Syclop.h:364
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:124
DirectedControlSamplerPtr allocDirectedControlSampler() const
Allocate an instance of the DirectedControlSampler to use. This will be the default (SimpleDirectedCo...
Definition of an abstract state.
Definition: State.h:113
Control * control
The control contained by the motion.
Definition: Syclop.h:366
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Syclop.cpp:64
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Syclop.cpp:49
Representation of a motion.
Definition: Syclop.h:352
const SpaceInformation * siC_
Handle to the control::SpaceInformation object.
Definition: Syclop.h:480
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SyclopRRT.cpp:41
Representation of a region in the Decomposition assigned to Syclop.
Definition: Syclop.h:374
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: SyclopRRT.h:212
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
int index
The index of the graph node corresponding to this region.
Definition: Syclop.h:408
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
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...
void freeMemory()
Free the memory allocated by this planner.
Definition: SyclopRRT.cpp:171
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: SyclopRRT.h:201
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SyclopRRT.cpp:60
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
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 addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:122
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
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance.