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/control/spaces/DiscreteControlSpace.h"
39 #include "ompl/geometric/PathGeometric.h"
40 #include "ompl/base/samplers/UniformValidStateSampler.h"
41 #include "ompl/util/Exception.h"
42 #include "ompl/util/Console.h"
43 #include <numeric>
44 #include <cmath>
45 
46 namespace
47 {
48  unsigned int getNumberOfDiscreteControls(const ompl::control::ControlSpace* cs)
49  {
50  if (cs->isCompound())
51  {
54  unsigned int num = 0;
55  for (unsigned int i = 0; i < ccs->getSubspaceCount(); ++i)
56  num += getNumberOfDiscreteControls(ccs->getSubspace(i).get());
57 
58  return num;
59  }
60  else
61  if (dynamic_cast<const ompl::control::DiscreteControlSpace*>(cs))
62  return 1;
63  return 0;
64  }
65 
66  void printDiscreteControls(std::ostream &out, const ompl::control::ControlSpace* cs,
67  const ompl::control::Control* c)
68  {
69  if (cs->isCompound())
70  {
73  for (unsigned int i = 0; i < ccs->getSubspaceCount(); ++i)
74  printDiscreteControls(out, ccs->getSubspace(i).get(),
75  c->as<ompl::control::CompoundControl>()->components[i]);
76  }
77  else if (dynamic_cast<const ompl::control::DiscreteControlSpace*>(cs))
79  }
80 }
81 
83 {
84  if (!dynamic_cast<const SpaceInformation*>(si_.get()))
85  throw Exception("Cannot create a path with controls from a space that does not support controls");
86 }
87 
88 ompl::control::PathControl::PathControl(const PathControl &path) : base::Path(path.si_)
89 {
90  copyFrom(path);
91 }
92 
94 {
95  PathControl pc(*this);
96  pc.interpolate();
98  pg.getStates().swap(pc.states_);
99  return pg;
100 }
101 
103 {
104  freeMemory();
105  si_ = other.si_;
106  copyFrom(other);
107  return *this;
108 }
109 
111 {
112  states_.resize(other.states_.size());
113  controls_.resize(other.controls_.size());
114 
115  for (unsigned int i = 0 ; i < states_.size() ; ++i)
116  states_[i] = si_->cloneState(other.states_[i]);
117 
118  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
119  for (unsigned int i = 0 ; i < controls_.size() ; ++i)
120  controls_[i] = si->cloneControl(other.controls_[i]);
121 
122  controlDurations_ = other.controlDurations_;
123 }
124 
126 {
127  return std::accumulate(controlDurations_.begin(), controlDurations_.end(), 0.0);
128 }
129 
130 void ompl::control::PathControl::print(std::ostream &out) const
131 {
132  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
133  double res = si->getPropagationStepSize();
134  out << "Control path with " << states_.size() << " states" << std::endl;
135  for (unsigned int i = 0 ; i < controls_.size() ; ++i)
136  {
137  out << "At state ";
138  si_->printState(states_[i], out);
139  out << " apply control ";
140  si->printControl(controls_[i], out);
141  out << " for " << (int)floor(0.5 + controlDurations_[i]/res) << " steps" << std::endl;
142  }
143  out << "Arrive at state ";
144  si_->printState(states_[controls_.size()], out);
145  out << std::endl;
146 }
147 
148 void ompl::control::PathControl::printAsMatrix(std::ostream &out) const
149 {
150  if (states_.empty())
151  return;
152  const base::StateSpace* space(si_->getStateSpace().get());
153  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
154  const ControlSpace* cspace(si->getControlSpace().get());
155  std::vector<double> reals;
156 
157  space->copyToReals(reals, states_[0]);
158  std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out, " "));
159  if (controls_.empty())
160  return;
161 
162  const ControlSpace* cs = static_cast<const SpaceInformation*>(si_.get())->getControlSpace().get();
163  unsigned int n = 0, m = getNumberOfDiscreteControls(cs);
164  double* val;
165  while ((val = cspace->getValueAddressAtIndex(controls_[0], n)))
166  ++n;
167  for (unsigned int i = 0 ; i < n + m; ++i)
168  out << "0 ";
169  out << '0' << std::endl;
170  for (unsigned int i = 0 ; i < controls_.size(); ++i)
171  {
172  space->copyToReals(reals, states_[i + 1]);
173  std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out, " "));
174  // print discrete controls
175  printDiscreteControls(out, cs, controls_[i]);
176  // print real-valued controls
177  for (unsigned int j = 0; j < n; ++j)
178  out << *cspace->getValueAddressAtIndex(controls_[i], j) << ' ';
179  out << controlDurations_[i] << std::endl;
180  }
181 }
182 
184 {
185  if (states_.size() <= controls_.size())
186  {
187  OMPL_ERROR("Interpolation not performed. Number of states in the path should be strictly greater than the number of controls.");
188  return;
189  }
190 
191  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
192  std::vector<base::State*> newStates;
193  std::vector<Control*> newControls;
194  std::vector<double> newControlDurations;
195 
196  double res = si->getPropagationStepSize();
197  for (unsigned int i = 0 ; i < controls_.size() ; ++i)
198  {
199  int steps = (int)floor(0.5 + controlDurations_[i] / res);
200  assert(steps >= 0);
201  if (steps <= 1)
202  {
203  newStates.push_back(states_[i]);
204  newControls.push_back(controls_[i]);
205  newControlDurations.push_back(controlDurations_[i]);
206  continue;
207  }
208  std::vector<base::State*> istates;
209  si->propagate(states_[i], controls_[i], steps, istates, true);
210  // last state is already in the non-interpolated path
211  if (!istates.empty())
212  {
213  si_->freeState(istates.back());
214  istates.pop_back();
215  }
216  newStates.push_back(states_[i]);
217  newStates.insert(newStates.end(), istates.begin(), istates.end());
218  newControls.push_back(controls_[i]);
219  newControlDurations.push_back(res);
220  for (int j = 1 ; j < steps; ++j)
221  {
222  newControls.push_back(si->cloneControl(controls_[i]));
223  newControlDurations.push_back(res);
224  }
225  }
226  newStates.push_back(states_[controls_.size()]);
227  states_.swap(newStates);
228  controls_.swap(newControls);
229  controlDurations_.swap(newControlDurations);
230 }
231 
233 {
234  if (controls_.empty())
235  {
236  if (states_.size() == 1)
237  return si_->isValid(states_[0]);
238  else
239  return false;
240  }
241 
242  bool valid = true;
243  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
244  double res = si->getPropagationStepSize();
245  base::State *next = si_->allocState();
246  for (unsigned int i = 0 ; valid && i < controls_.size() ; ++i)
247  {
248  unsigned int steps = (unsigned int)floor(0.5 + controlDurations_[i] / res);
249  if (!si->isValid(states_[i]) ||
250  si->propagateWhileValid(states_[i], controls_[i], steps, next) != steps ||
251  si->distance(next, states_[i + 1]) > std::numeric_limits<float>::epsilon())
252  valid = false;
253  }
254  si_->freeState(next);
255 
256  return valid;
257 }
258 
260 {
261  states_.push_back(si_->cloneState(state));
262 }
263 
264 void ompl::control::PathControl::append(const base::State *state, const Control *control, double duration)
265 {
266  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
267  states_.push_back(si->cloneState(state));
268  controls_.push_back(si->cloneControl(control));
269  controlDurations_.push_back(duration);
270 }
271 
273 {
274  freeMemory();
275  states_.resize(2);
276  controlDurations_.resize(1);
277  controls_.resize(1);
278 
279  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
280  states_[0] = si->allocState();
281  states_[1] = si->allocState();
282  controls_[0] = si->allocControl();
283 
285  ss->sampleUniform(states_[0]);
287  cs->sample(controls_[0], states_[0]);
288  unsigned int steps = cs->sampleStepCount(si->getMinControlDuration(), si->getMaxControlDuration());
289  controlDurations_[0] = steps * si->getPropagationStepSize();
290  si->propagate(states_[0], controls_[0], steps, states_[1]);
291 }
292 
293 bool ompl::control::PathControl::randomValid(unsigned int attempts)
294 {
295  freeMemory();
296  states_.resize(2);
297  controlDurations_.resize(1);
298  controls_.resize(1);
299 
300  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
301  states_[0] = si->allocState();
302  states_[1] = si->allocState();
303  controls_[0] = si->allocControl();
304 
307  uvss->setNrAttempts(attempts);
308  bool ok = false;
309  for (unsigned int i = 0 ; i < attempts ; ++i)
310  if (uvss->sample(states_[0]))
311  {
312  cs->sample(controls_[0], states_[0]);
313  unsigned int steps = cs->sampleStepCount(si->getMinControlDuration(), si->getMaxControlDuration());
314  controlDurations_[0] = steps * si->getPropagationStepSize();
315  if (si->propagateWhileValid(states_[0], controls_[0], steps, states_[1]) == steps)
316  {
317  ok = true;
318  break;
319  }
320  }
321  delete uvss;
322 
323  if (!ok)
324  {
325  freeMemory();
326  states_.clear();
327  controls_.clear();
328  controlDurations_.clear();
329  }
330  return ok;
331 }
332 
334 {
335  for (unsigned int i = 0 ; i < states_.size() ; ++i)
336  si_->freeState(states_[i]);
337  const SpaceInformation *si = static_cast<const SpaceInformation*>(si_.get());
338  for (unsigned int i = 0 ; i < controls_.size() ; ++i)
339  si->freeControl(controls_[i]);
340 }
unsigned int getMaxControlDuration(void) const
Get the maximum number of steps a control is propagated for.
double distance(const State *state1, const State *state2) const
Compute the distance between two states.
virtual void printAsMatrix(std::ostream &out) const
Print the path as a real-valued matrix where the i-th row represents the i-th state along the path...
PathControl & operator=(const PathControl &other)
Assignment operator.
ControlSamplerPtr allocControlSampler(void) const
Allocate a control sampler.
Control * allocControl(void) const
Allocate memory for a control.
virtual bool sample(State *state)
Sample a state. Return false in case of failure.
void freeMemory(void)
Free the memory allocated by the path.
unsigned int propagateWhileValid(const base::State *state, const Control *control, int steps, base::State *result) const
Propagate the model of the system forward, starting at a given state, with a given control...
void append(const base::State *state)
Append state to the end of the path; it is assumed state is the first state, so no control is applied...
Definition of an abstract control.
Definition: Control.h:48
A boost shared pointer wrapper for ompl::base::StateSampler.
A boost shared pointer wrapper for ompl::control::ControlSampler.
bool isValid(const State *state) const
Check if a given state is valid or not.
void propagate(const base::State *state, const Control *control, int steps, base::State *result) const
Propagate the model of the system forward, starting a a given state, with a given control...
const ControlSpacePtr & getSubspace(const unsigned int index) const
Get a specific subspace from the compound control space.
unsigned int getSubspaceCount(void) const
Get the number of control spaces that make up the compound control space.
The definition of a discrete control.
std::vector< base::State * > & getStates(void)
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
virtual double length(void) const
The path length (sum of control durations)
void freeControl(Control *control) const
Free the memory of a control.
Definition of a control path.
Definition: PathControl.h:54
virtual void print(std::ostream &out) const
Print the path to a stream.
State * cloneState(const State *source) const
Clone a state.
A state sampler that only samples valid states, uniformly.
SpaceInformationPtr si_
The space information this path is part of.
Definition: Path.h:103
geometric::PathGeometric asGeometric(void) const
Convert this path into a geometric path (interpolation is performed and then states are copied) ...
Definition: PathControl.cpp:93
int value
The current control - an int in range [lowerBound, upperBound].
StateSamplerPtr allocStateSampler(void) const
Allocate a uniform state sampler for the state space.
double getPropagationStepSize(void) const
Propagation is performed at integer multiples of a specified step size. This function returns the val...
Definition of a compound control.
Definition: Control.h:93
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
void random(void)
Set this path to a random segment.
bool randomValid(unsigned int attempts)
Set this path to a random valid segment. Sample attempts times for valid segments. Returns true on success.
void interpolate(void)
Make the path such that all controls are applied for a single time step (computes intermediate states...
A control space representing the space of applicable controls.
Definition: ControlSpace.h:66
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:73
Definition of an abstract state.
Definition: State.h:50
The exception type for ompl.
Definition: Exception.h:47
std::vector< double > controlDurations_
The duration of the control applied at each state. This array contains one element less than the list...
Definition: PathControl.h:190
std::vector< base::State * > states_
The list of states that make up the path.
Definition: PathControl.h:184
const T * as(void) const
Cast this instance to a desired type.
Definition: Control.h:72
A control space to allow the composition of control spaces.
Definition: ControlSpace.h:196
PathControl(const base::SpaceInformationPtr &si)
Constructor.
Definition: PathControl.cpp:82
virtual bool check(void) const
Check if the path is valid.
unsigned int getMinControlDuration(void) const
Get the minimum number of steps a control is propagated for.
virtual bool isCompound(void) const
Check if the control space is compound.
Definition of a geometric path.
Definition: PathGeometric.h:55
Control * cloneControl(const Control *source) const
Clone a control.
Space information containing necessary information for planning with controls. setup() needs to be ca...
void copyFrom(const PathControl &other)
Copy the content of a path to this one.
T * as(void)
Cast this instance to a desired type.
Definition: ControlSpace.h:77
void printControl(const Control *control, std::ostream &out=std::cout) const
Print a control to a stream.
std::vector< Control * > controls_
The control applied at each state. This array contains one element less than the list of states...
Definition: PathControl.h:187
State * allocState(void) const
Allocate memory for a state.
void setNrAttempts(unsigned int attempts)
Finding a valid sample usually requires performing multiple attempts. This call allows setting the nu...