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  {
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 if (dynamic_cast<const ompl::control::DiscreteControlSpace *>(cs))
61  return 1;
62  return 0;
63  }
64 
65  void printDiscreteControls(std::ostream &out, const ompl::control::ControlSpace *cs,
66  const ompl::control::Control *c)
67  {
68  if (cs->isCompound())
69  {
71  for (unsigned int i = 0; i < ccs->getSubspaceCount(); ++i)
72  printDiscreteControls(out, ccs->getSubspace(i).get(),
73  c->as<ompl::control::CompoundControl>()->components[i]);
74  }
75  else if (dynamic_cast<const ompl::control::DiscreteControlSpace *>(cs))
77  }
78 }
79 
81 {
82  if (!dynamic_cast<const SpaceInformation *>(si_.get()))
83  throw Exception("Cannot create a path with controls from a space that does not support controls");
84 }
85 
87 {
88  copyFrom(path);
89 }
90 
92 {
93  PathControl pc(*this);
94  pc.interpolate();
96  pg.getStates().swap(pc.states_);
97  return pg;
98 }
99 
101 {
102  freeMemory();
103  si_ = other.si_;
104  copyFrom(other);
105  return *this;
106 }
107 
109 {
110  states_.resize(other.states_.size());
111  controls_.resize(other.controls_.size());
112 
113  for (unsigned int i = 0; i < states_.size(); ++i)
114  states_[i] = si_->cloneState(other.states_[i]);
115 
116  const SpaceInformation *si = static_cast<const SpaceInformation *>(si_.get());
117  for (unsigned int i = 0; i < controls_.size(); ++i)
118  controls_[i] = si->cloneControl(other.controls_[i]);
119 
121 }
122 
124 {
125  OMPL_ERROR("Error: Cost computation is only implemented for paths of type PathGeometric.");
126  return opt->identityCost();
127 }
128 
130 {
131  return std::accumulate(controlDurations_.begin(), controlDurations_.end(), 0.0);
132 }
133 
134 void ompl::control::PathControl::print(std::ostream &out) const
135 {
136  const SpaceInformation *si = static_cast<const SpaceInformation *>(si_.get());
137  double res = si->getPropagationStepSize();
138  out << "Control path with " << states_.size() << " states" << std::endl;
139  for (unsigned int i = 0; i < controls_.size(); ++i)
140  {
141  out << "At state ";
142  si_->printState(states_[i], out);
143  out << " apply control ";
144  si->printControl(controls_[i], out);
145  out << " for " << (int)floor(0.5 + controlDurations_[i] / res) << " steps" << std::endl;
146  }
147  out << "Arrive at state ";
148  si_->printState(states_[controls_.size()], out);
149  out << std::endl;
150 }
151 
152 void ompl::control::PathControl::printAsMatrix(std::ostream &out) const
153 {
154  if (states_.empty())
155  return;
156  const base::StateSpace *space(si_->getStateSpace().get());
157  const SpaceInformation *si = static_cast<const SpaceInformation *>(si_.get());
158  const ControlSpace *cspace(si->getControlSpace().get());
159  std::vector<double> reals;
160 
161  space->copyToReals(reals, states_[0]);
162  std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out, " "));
163  if (controls_.empty())
164  return;
165 
166  const ControlSpace *cs = static_cast<const SpaceInformation *>(si_.get())->getControlSpace().get();
167  unsigned int n = 0, m = getNumberOfDiscreteControls(cs);
168  double *val;
169  while ((val = cspace->getValueAddressAtIndex(controls_[0], n)))
170  ++n;
171  for (unsigned int i = 0; i < n + m; ++i)
172  out << "0 ";
173  out << '0' << std::endl;
174  for (unsigned int i = 0; i < controls_.size(); ++i)
175  {
176  space->copyToReals(reals, states_[i + 1]);
177  std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out, " "));
178  // print discrete controls
179  printDiscreteControls(out, cs, controls_[i]);
180  // print real-valued controls
181  for (unsigned int j = 0; j < n; ++j)
182  out << *cspace->getValueAddressAtIndex(controls_[i], j) << ' ';
183  out << controlDurations_[i] << std::endl;
184  }
185 }
186 
188 {
189  if (states_.size() <= controls_.size())
190  {
191  OMPL_ERROR("Interpolation not performed. Number of states in the path should be strictly greater than the "
192  "number of controls.");
193  return;
194  }
195 
196  const SpaceInformation *si = static_cast<const SpaceInformation *>(si_.get());
197  std::vector<base::State *> newStates;
198  std::vector<Control *> newControls;
199  std::vector<double> newControlDurations;
200 
201  double res = si->getPropagationStepSize();
202  for (unsigned int i = 0; i < controls_.size(); ++i)
203  {
204  int steps = (int)floor(0.5 + controlDurations_[i] / res);
205  assert(steps >= 0);
206  if (steps <= 1)
207  {
208  newStates.push_back(states_[i]);
209  newControls.push_back(controls_[i]);
210  newControlDurations.push_back(controlDurations_[i]);
211  continue;
212  }
213  std::vector<base::State *> istates;
214  si->propagate(states_[i], controls_[i], steps, istates, true);
215  // last state is already in the non-interpolated path
216  if (!istates.empty())
217  {
218  si_->freeState(istates.back());
219  istates.pop_back();
220  }
221  newStates.push_back(states_[i]);
222  newStates.insert(newStates.end(), istates.begin(), istates.end());
223  newControls.push_back(controls_[i]);
224  newControlDurations.push_back(res);
225  for (int j = 1; j < steps; ++j)
226  {
227  newControls.push_back(si->cloneControl(controls_[i]));
228  newControlDurations.push_back(res);
229  }
230  }
231  newStates.push_back(states_[controls_.size()]);
232  states_.swap(newStates);
233  controls_.swap(newControls);
234  controlDurations_.swap(newControlDurations);
235 }
236 
238 {
239  if (controls_.empty())
240  {
241  if (states_.size() == 1)
242  return si_->isValid(states_[0]);
243  else
244  return false;
245  }
246 
247  bool valid = true;
248  const SpaceInformation *si = static_cast<const SpaceInformation *>(si_.get());
249  double res = si->getPropagationStepSize();
250  base::State *next = si_->allocState();
251  for (unsigned int i = 0; valid && i < controls_.size(); ++i)
252  {
253  unsigned int steps = (unsigned int)floor(0.5 + controlDurations_[i] / res);
254  if (!si->isValid(states_[i]) || si->propagateWhileValid(states_[i], controls_[i], steps, next) != steps ||
255  si->distance(next, states_[i + 1]) > std::numeric_limits<float>::epsilon())
256  valid = false;
257  }
258  si_->freeState(next);
259 
260  return valid;
261 }
262 
264 {
265  states_.push_back(si_->cloneState(state));
266 }
267 
268 void ompl::control::PathControl::append(const base::State *state, const Control *control, double duration)
269 {
270  const SpaceInformation *si = static_cast<const SpaceInformation *>(si_.get());
271  states_.push_back(si->cloneState(state));
272  controls_.push_back(si->cloneControl(control));
273  controlDurations_.push_back(duration);
274 }
275 
277 {
278  freeMemory();
279  states_.resize(2);
280  controlDurations_.resize(1);
281  controls_.resize(1);
282 
283  const SpaceInformation *si = static_cast<const SpaceInformation *>(si_.get());
284  states_[0] = si->allocState();
285  states_[1] = si->allocState();
286  controls_[0] = si->allocControl();
287 
289  ss->sampleUniform(states_[0]);
291  cs->sample(controls_[0], states_[0]);
292  unsigned int steps = cs->sampleStepCount(si->getMinControlDuration(), si->getMaxControlDuration());
293  controlDurations_[0] = steps * si->getPropagationStepSize();
294  si->propagate(states_[0], controls_[0], steps, states_[1]);
295 }
296 
297 bool ompl::control::PathControl::randomValid(unsigned int attempts)
298 {
299  freeMemory();
300  states_.resize(2);
301  controlDurations_.resize(1);
302  controls_.resize(1);
303 
304  const SpaceInformation *si = static_cast<const SpaceInformation *>(si_.get());
305  states_[0] = si->allocState();
306  states_[1] = si->allocState();
307  controls_[0] = si->allocControl();
308 
310  auto uvss(std::make_shared<base::UniformValidStateSampler>(si));
311  uvss->setNrAttempts(attempts);
312  bool ok = false;
313  for (unsigned int i = 0; i < attempts; ++i)
314  if (uvss->sample(states_[0]))
315  {
316  cs->sample(controls_[0], states_[0]);
317  unsigned int steps = cs->sampleStepCount(si->getMinControlDuration(), si->getMaxControlDuration());
318  controlDurations_[0] = steps * si->getPropagationStepSize();
319  if (si->propagateWhileValid(states_[0], controls_[0], steps, states_[1]) == steps)
320  {
321  ok = true;
322  break;
323  }
324  }
325 
326  if (!ok)
327  {
328  freeMemory();
329  states_.clear();
330  controls_.clear();
331  controlDurations_.clear();
332  }
333  return ok;
334 }
335 
337 {
338  for (auto &state : states_)
339  si_->freeState(state);
340  const SpaceInformation *si = static_cast<const SpaceInformation *>(si_.get());
341  for (auto &control : controls_)
342  si->freeControl(control);
343 }
PathControl & operator=(const PathControl &other)
Assignment operator.
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:64
void print(std::ostream &out) const override
Print the path to a stream.
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:47
A shared pointer wrapper for ompl::base::StateSampler.
A 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.
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:91
ControlSamplerPtr allocControlSampler() const
Allocate a control sampler.
std::vector< Control * > controls_
The control applied at each state. This array contains one element less than the list of states...
Definition: PathControl.h:208
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.
SpaceInformationPtr si_
The space information this path is part of.
Definition: Path.h:123
int value
The current control - an int in range [lowerBound, upperBound].
T * as()
Cast this instance to a desired type.
Definition: ControlSpace.h:77
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:84
#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.
base::Cost cost(const base::OptimizationObjectivePtr &obj) const override
Not yet implemented.
bool check() const override
Check if the path is valid.
void random()
Set this path to a random segment.
A control space representing the space of applicable controls.
Definition: ControlSpace.h:63
A 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:70
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:49
The exception type for ompl.
Definition: Exception.h:46
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:212
A shared pointer wrapper for ompl::base::OptimizationObjective.
A control space to allow the composition of control spaces.
Definition: ControlSpace.h:198
PathControl(const base::SpaceInformationPtr &si)
Constructor.
Definition: PathControl.cpp:80
double length() const override
The path length (sum of control durations)
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.
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.
std::vector< base::State * > states_
The list of states that make up the path.
Definition: PathControl.h:204
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...
void printControl(const Control *control, std::ostream &out=std::cout) const
Print a control to a stream.