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