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 }
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
unsigned int getMinControlDuration() const
Get the minimum number of steps a control is propagated for.
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
virtual bool hasControls() const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
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
void freeControl(Control *control) const
Free the memory of a control.
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
DirectedControlSamplerPtr allocDirectedControlSampler() const
Allocate an instance of the DirectedControlSampler to use. This will be the default (SimpleDirectedCo...
double getPropagationStepSize() const
Propagation is performed at integer multiples of a specified step size. This function returns the val...
void freeMemory()
Free the memory allocated by this planner.
Definition: SyclopRRT.cpp:169
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
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: SyclopRRT.h:96
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
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
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
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...
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