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