All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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_)
75  data.addGoalVertex (base::PlannerDataVertex(lastGoalMotion_->state));
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  decomp_->sampleFromRegion(region.index, sampler, rmotion->state);
110 
111  Motion* nmotion;
112  if (regionalNN_)
113  {
114  /* Instead of querying the nearest neighbors datastructure over the entire tree of motions,
115  * here we perform a linear search over all motions in the selected region and its neighbors. */
116  std::vector<int> searchRegions;
117  decomp_->getNeighbors(region.index, searchRegions);
118  searchRegions.push_back(region.index);
119 
120  std::vector<Motion*> motions;
121  for (std::vector<int>::const_iterator i = searchRegions.begin(); i != searchRegions.end(); ++i)
122  {
123  const std::vector<Motion*>& regionMotions = getRegionFromIndex(*i).motions;
124  motions.insert(motions.end(), regionMotions.begin(), regionMotions.end());
125  }
126 
127  std::vector<Motion*>::const_iterator i = motions.begin();
128  nmotion = *i;
129  double minDistance = distanceFunction(rmotion, nmotion);
130  ++i;
131  while (i != motions.end())
132  {
133  Motion* m = *i;
134  const double dist = distanceFunction(rmotion, m);
135  if (dist < minDistance)
136  {
137  nmotion = m;
138  minDistance = dist;
139  }
140  ++i;
141  }
142  }
143  else
144  {
145  assert (nn_);
146  nmotion = nn_->nearest(rmotion);
147  }
148 
149  base::State* newState = si_->allocState();
150 
151  unsigned int duration = controlSampler_->sampleTo(rmotion->control, nmotion->control, nmotion->state, rmotion->state);
152 
153  duration = siC_->propagateWhileValid(nmotion->state, rmotion->control, duration, newState);
154 
155  if (duration >= siC_->getMinControlDuration())
156  {
157  Motion* motion = new Motion(siC_);
158  si_->copyState(motion->state, newState);
159  siC_->copyControl(motion->control, rmotion->control);
160  motion->steps = duration;
161  motion->parent = nmotion;
162  newMotions.push_back(motion);
163  if (nn_)
164  nn_->add(motion);
165  lastGoalMotion_ = motion;
166  }
167 
168  si_->freeState(rmotion->state);
169  siC_->freeControl(rmotion->control);
170  delete rmotion;
171  si_->freeState(newState);
172 }
173 
175 {
176  if (nn_)
177  {
178  std::vector<Motion*> motions;
179  nn_->list(motions);
180  for (std::vector<Motion*>::iterator i = motions.begin(); i != motions.end(); ++i)
181  {
182  Motion* m = *i;
183  if (m->state)
184  si_->freeState(m->state);
185  if (m->control)
186  siC_->freeControl(m->control);
187  delete m;
188  }
189  }
190 }