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_ = NULL;
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*>(si_->getStateSpace()));
53  nn_->setDistanceFunction(boost::bind(&SyclopRRT::distanceFunction, this, _1, _2));
54  }
55 }
56 
58 {
59  Syclop::clear();
60  freeMemory();
61  if (nn_)
62  nn_->clear();
63  lastGoalMotion_ = NULL;
64 }
65 
67 {
68  Planner::getPlannerData(data);
69  std::vector<Motion*> motions;
70  if (nn_)
71  nn_->list(motions);
72  double delta = siC_->getPropagationStepSize();
73 
74  if (lastGoalMotion_)
76 
77  for (size_t i = 0; i < motions.size(); ++i)
78  {
79  if (motions[i]->parent)
80  {
81  if (data.hasControls())
82  data.addEdge (base::PlannerDataVertex(motions[i]->parent->state),
83  base::PlannerDataVertex(motions[i]->state),
84  control::PlannerDataEdgeControl (motions[i]->control, motions[i]->steps * delta));
85  else
86  data.addEdge (base::PlannerDataVertex(motions[i]->parent->state),
87  base::PlannerDataVertex(motions[i]->state));
88  }
89  else
90  data.addStartVertex (base::PlannerDataVertex(motions[i]->state));
91  }
92 }
93 
95 {
96  Motion *motion = new Motion(siC_);
97  si_->copyState(motion->state, s);
98  siC_->nullControl(motion->control);
99 
100  if (nn_)
101  nn_->add(motion);
102  return motion;
103 }
104 
105 void ompl::control::SyclopRRT::selectAndExtend(Region &region, std::vector<Motion*>& newMotions)
106 {
107  Motion *rmotion = new Motion(siC_);
108  base::StateSamplerPtr sampler(si_->allocStateSampler());
109  std::vector<double> coord(decomp_->getDimension());
110  decomp_->sampleFromRegion(region.index, rng_, coord);
111  decomp_->sampleFullState(sampler, coord, rmotion->state);
112 
113  Motion *nmotion;
114  if (regionalNN_)
115  {
116  /* Instead of querying the nearest neighbors datastructure over the entire tree of motions,
117  * here we perform a linear search over all motions in the selected region and its neighbors. */
118  std::vector<int> searchRegions;
119  decomp_->getNeighbors(region.index, searchRegions);
120  searchRegions.push_back(region.index);
121 
122  std::vector<Motion*> motions;
123  for (std::vector<int>::const_iterator i = searchRegions.begin(); i != searchRegions.end(); ++i)
124  {
125  const std::vector<Motion*>& regionMotions = getRegionFromIndex(*i).motions;
126  motions.insert(motions.end(), regionMotions.begin(), regionMotions.end());
127  }
128 
129  std::vector<Motion*>::const_iterator i = motions.begin();
130  nmotion = *i;
131  double minDistance = distanceFunction(rmotion, nmotion);
132  ++i;
133  while (i != motions.end())
134  {
135  Motion *m = *i;
136  const double dist = distanceFunction(rmotion, m);
137  if (dist < minDistance)
138  {
139  nmotion = m;
140  minDistance = dist;
141  }
142  ++i;
143  }
144  }
145  else
146  {
147  assert (nn_);
148  nmotion = nn_->nearest(rmotion);
149  }
150 
151  unsigned int duration = controlSampler_->sampleTo(rmotion->control, nmotion->control, nmotion->state, rmotion->state);
152  if (duration >= siC_->getMinControlDuration())
153  {
154  rmotion->steps = duration;
155  rmotion->parent = nmotion;
156  newMotions.push_back(rmotion);
157  if (nn_)
158  nn_->add(rmotion);
159  lastGoalMotion_ = rmotion;
160  }
161  else
162  {
163  si_->freeState(rmotion->state);
164  siC_->freeControl(rmotion->control);
165  delete rmotion;
166  }
167 }
168 
170 {
171  if (nn_)
172  {
173  std::vector<Motion*> motions;
174  nn_->list(motions);
175  for (std::vector<Motion*>::iterator i = motions.begin(); i != motions.end(); ++i)
176  {
177  Motion *m = *i;
178  if (m->state)
179  si_->freeState(m->state);
180  if (m->control)
181  siC_->freeControl(m->control);
182  delete m;
183  }
184  }
185 }
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:164
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Syclop.cpp:48
base::State * state
The state contained by the motion.
Definition: Syclop.h:265
virtual void selectAndExtend(Region &region, std::vector< Motion *> &newMotions)
Select a Motion from the given Region, and extend the tree from the Motion. Add any new motions creat...
Definition: SyclopRRT.cpp:105
double getPropagationStepSize() const
Propagation is performed at integer multiples of a specified step size. This function returns the val...
unsigned int getMinControlDuration() const
Get the minimum number of steps a control is propagated for.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: SyclopRRT.cpp:66
void nullControl(Control *control) const
Make the control have no effect if it were to be applied to a state for any amount of time...
int index
The index of the graph node corresponding to this region.
Definition: Syclop.h:317
RNG rng_
Random number generator.
Definition: Syclop.h:399
A boost shared pointer wrapper for ompl::base::StateSampler.
Representation of a region in the Decomposition assigned to Syclop.
Definition: Syclop.h:277
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 Syclop::Motion * addRoot(const base::State *s)
Add State s as a new root in the low-level tree, and return the Motion corresponding to s...
Definition: SyclopRRT.cpp:94
std::vector< Motion * > motions
The tree motions contained in this region.
Definition: Syclop.h:305
const SpaceInformation * siC_
Handle to the control::SpaceInformation object.
Definition: Syclop.h:393
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
Definition: PlannerData.h:60
DecompositionPtr decomp_
The high level decomposition used to focus tree expansion.
Definition: Syclop.h:396
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: SyclopRRT.h:107
Representation of a motion.
Definition: Syclop.h:251
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SyclopRRT.cpp:41
Control * control
The control contained by the motion.
Definition: Syclop.h:267
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Syclop.cpp:57
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
void freeMemory()
Free the memory allocated by this planner.
Definition: SyclopRRT.cpp:169
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SyclopRRT.cpp:57
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:50
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: SyclopRRT.h:96
const Region & getRegionFromIndex(const int rid) const
Returns a reference to the Region object with the given index. Assumes the index is valid...
Definition: Syclop.h:369
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
void freeControl(Control *control) const
Free the memory of a control.
const Motion * parent
The parent motion in the tree.
Definition: Syclop.h:269
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
unsigned int steps
The number of steps for which the control is applied.
Definition: Syclop.h:271