All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
SyclopEST.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/SyclopEST.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 
41 {
42  Syclop::setup();
43  sampler_ = si_->allocStateSampler();
44  controlSampler_ = siC_->allocControlSampler();
45  lastGoalMotion_ = NULL;
46 }
47 
49 {
50  Syclop::clear();
51  freeMemory();
52  motions_.clear();
53  lastGoalMotion_ = NULL;
54 }
55 
57 {
58  Planner::getPlannerData(data);
59 
60  double delta = siC_->getPropagationStepSize();
61 
62  if (lastGoalMotion_)
63  data.addGoalVertex(lastGoalMotion_->state);
64 
65  for (size_t i = 0; i < motions_.size(); ++i)
66  {
67  if (motions_[i]->parent)
68  {
69  if (data.hasControls())
70  data.addEdge (base::PlannerDataVertex(motions_[i]->parent->state),
71  base::PlannerDataVertex(motions_[i]->state),
72  control::PlannerDataEdgeControl (motions_[i]->control, motions_[i]->steps * delta));
73  else
74  data.addEdge (base::PlannerDataVertex(motions_[i]->parent->state),
75  base::PlannerDataVertex(motions_[i]->state));
76  }
77  else
78  data.addStartVertex (base::PlannerDataVertex(motions_[i]->state));
79  }
80 }
81 
83 {
84  Motion* motion = new Motion(siC_);
85  si_->copyState(motion->state, s);
86  siC_->nullControl(motion->control);
87  motions_.push_back(motion);
88  return motion;
89 }
90 
91 void ompl::control::SyclopEST::selectAndExtend(Region& region, std::vector<Motion*>& newMotions)
92 {
93  Motion* treeMotion = region.motions[rng_.uniformInt(0, region.motions.size()-1)];
94  Control* rctrl = siC_->allocControl();
95  base::State* newState = si_->allocState();
96 
97  controlSampler_->sample(rctrl, treeMotion->state);
98  unsigned int duration = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration());
99  duration = siC_->propagateWhileValid(treeMotion->state, rctrl, duration, newState);
100 
101  if (duration >= siC_->getMinControlDuration())
102  {
103  Motion* motion = new Motion(siC_);
104  si_->copyState(motion->state, newState);
105  siC_->copyControl(motion->control, rctrl);
106  motion->steps = duration;
107  motion->parent = treeMotion;
108  motions_.push_back(motion);
109  newMotions.push_back(motion);
110 
111  lastGoalMotion_ = motion;
112  }
113 
114  siC_->freeControl(rctrl);
115  si_->freeState(newState);
116 }
117 
119 {
120  for (std::vector<Motion*>::iterator i = motions_.begin(); i != motions_.end(); ++i)
121  {
122  Motion* m = *i;
123  if (m->state)
124  si_->freeState(m->state);
125  if (m->control)
126  siC_->freeControl(m->control);
127  delete m;
128  }
129 }