All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PathControl.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, 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: Ioan Sucan */
36 
37 #include "ompl/control/PathControl.h"
38 #include "ompl/geometric/PathGeometric.h"
39 #include "ompl/base/samplers/UniformValidStateSampler.h"
40 #include "ompl/util/Exception.h"
41 #include "ompl/util/Console.h"
42 #include <numeric>
43 #include <cmath>
44 
46 {
47  if (!dynamic_cast<const SpaceInformation*>(si_.get()))
48  throw Exception("Cannot create a path with controls from a space that does not support controls");
49 }
50 
51 ompl::control::PathControl::PathControl(const PathControl &path) : base::Path(path.si_)
52 {
53  copyFrom(path);
54 }
55 
57 {
58  PathControl pc(*this);
59  pc.interpolate();
61  pg.getStates().swap(pc.states_);
62  return pg;
63 }
64 
66 {
67  freeMemory();
68  si_ = other.si_;
69  copyFrom(other);
70  return *this;
71 }
72 
74 {
75  states_.resize(other.states_.size());
76  controls_.resize(other.controls_.size());
77 
78  for (unsigned int i = 0 ; i < states_.size() ; ++i)
79  states_[i] = si_->cloneState(other.states_[i]);
80 
81  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
82  for (unsigned int i = 0 ; i < controls_.size() ; ++i)
83  controls_[i] = si->cloneControl(other.controls_[i]);
84 
85  controlDurations_ = other.controlDurations_;
86 }
87 
89 {
90  return std::accumulate(controlDurations_.begin(), controlDurations_.end(), 0.0);
91 }
92 
93 void ompl::control::PathControl::print(std::ostream &out) const
94 {
95  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
96  double res = si->getPropagationStepSize();
97  out << "Control path with " << states_.size() << " states" << std::endl;
98  for (unsigned int i = 0 ; i < controls_.size() ; ++i)
99  {
100  out << "At state ";
101  si_->printState(states_[i], out);
102  out << " apply control ";
103  si->printControl(controls_[i], out);
104  out << " for " << (int)floor(0.5 + controlDurations_[i]/res) << " steps" << std::endl;
105  }
106  out << "Arrive at state ";
107  si_->printState(states_[controls_.size()], out);
108  out << std::endl;
109 }
110 
112 {
113  if (states_.size() <= controls_.size())
114  {
115  logError("Interpolation not performed. Number of states in the path should be strictly greater than the number of controls.");
116  return;
117  }
118 
119  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
120  std::vector<base::State*> newStates;
121  std::vector<Control*> newControls;
122  std::vector<double> newControlDurations;
123 
124  double res = si->getPropagationStepSize();
125  for (unsigned int i = 0 ; i < controls_.size() ; ++i)
126  {
127  int steps = (int)floor(0.5 + controlDurations_[i] / res);
128  assert(steps >= 0);
129  if (steps <= 1)
130  {
131  newStates.push_back(states_[i]);
132  newControls.push_back(controls_[i]);
133  newControlDurations.push_back(controlDurations_[i]);
134  continue;
135  }
136  std::vector<base::State*> istates;
137  si->propagate(states_[i], controls_[i], steps, istates, true);
138  // last state is already in the non-interpolated path
139  if (!istates.empty())
140  {
141  si_->freeState(istates.back());
142  istates.pop_back();
143  }
144  newStates.push_back(states_[i]);
145  newStates.insert(newStates.end(), istates.begin(), istates.end());
146  newControls.push_back(controls_[i]);
147  newControlDurations.push_back(res);
148  for (int j = 1 ; j < steps; ++j)
149  {
150  newControls.push_back(si->cloneControl(controls_[i]));
151  newControlDurations.push_back(res);
152  }
153  }
154  newStates.push_back(states_[controls_.size()]);
155  states_.swap(newStates);
156  controls_.swap(newControls);
157  controlDurations_.swap(newControlDurations);
158 }
159 
161 {
162  if (controls_.empty())
163  {
164  if (states_.size() == 1)
165  return si_->isValid(states_[0]);
166  else
167  return false;
168  }
169 
170  bool valid = true;
171  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
172  double res = si->getPropagationStepSize();
173  base::State *dummy = si_->allocState();
174  for (unsigned int i = 0 ; valid && i < controls_.size() ; ++i)
175  {
176  unsigned int steps = (unsigned int)floor(0.5 + controlDurations_[i] / res);
177  if (!si->isValid(states_[i]) || si->propagateWhileValid(states_[i], controls_[i], steps, dummy) != steps)
178  valid = false;
179  }
180  si_->freeState(dummy);
181 
182  return valid;
183 }
184 
186 {
187  states_.push_back(si_->cloneState(state));
188 }
189 
190 void ompl::control::PathControl::append(const base::State *state, const Control *control, double duration)
191 {
192  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
193  states_.push_back(si->cloneState(state));
194  controls_.push_back(si->cloneControl(control));
195  controlDurations_.push_back(duration);
196 }
197 
199 {
200  freeMemory();
201  states_.resize(2);
202  controlDurations_.resize(1);
203  controls_.resize(1);
204 
205  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
206  states_[0] = si->allocState();
207  states_[1] = si->allocState();
208  controls_[0] = si->allocControl();
209 
211  ss->sampleUniform(states_[0]);
213  cs->sample(controls_[0], states_[0]);
214  unsigned int steps = cs->sampleStepCount(si->getMinControlDuration(), si->getMaxControlDuration());
215  controlDurations_[0] = steps * si->getPropagationStepSize();
216  si->propagate(states_[0], controls_[0], steps, states_[1]);
217 }
218 
219 bool ompl::control::PathControl::randomValid(unsigned int attempts)
220 {
221  freeMemory();
222  states_.resize(2);
223  controlDurations_.resize(1);
224  controls_.resize(1);
225 
226  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
227  states_[0] = si->allocState();
228  states_[1] = si->allocState();
229  controls_[0] = si->allocControl();
230 
233  uvss->setNrAttempts(attempts);
234  bool ok = false;
235  for (unsigned int i = 0 ; i < attempts ; ++i)
236  if (uvss->sample(states_[0]))
237  {
238  cs->sample(controls_[0], states_[0]);
239  unsigned int steps = cs->sampleStepCount(si->getMinControlDuration(), si->getMaxControlDuration());
240  controlDurations_[0] = steps * si->getPropagationStepSize();
241  if (si->propagateWhileValid(states_[0], controls_[0], steps, states_[1]) == steps)
242  {
243  ok = true;
244  break;
245  }
246  }
247  delete uvss;
248 
249  if (!ok)
250  {
251  freeMemory();
252  states_.clear();
253  controls_.clear();
254  controlDurations_.clear();
255  }
256  return ok;
257 }
258 
260 {
261  for (unsigned int i = 0 ; i < states_.size() ; ++i)
262  si_->freeState(states_[i]);
263  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
264  for (unsigned int i = 0 ; i < controls_.size() ; ++i)
265  si->freeControl(controls_[i]);
266 }