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  {
53  const auto *ccs = cs->as<ompl::control::CompoundControlSpace>();
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  if (dynamic_cast<const ompl::control::DiscreteControlSpace *>(cs) != nullptr)
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  {
70  const auto *ccs = cs->as<ompl::control::CompoundControlSpace>();
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) != nullptr)
77  }
78 }
79 
80 ompl::control::PathControl::PathControl(const base::SpaceInformationPtr &si) : base::Path(si)
81 {
82  if (dynamic_cast<const SpaceInformation *>(si_.get()) == nullptr)
83  throw Exception("Cannot create a path with controls from a space that does not support controls");
84 }
85 
86 ompl::control::PathControl::PathControl(const PathControl &path) : base::Path(path.si_)
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 auto *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 
120  controlDurations_ = other.controlDurations_;
121 }
122 
123 ompl::base::Cost ompl::control::PathControl::cost(const base::OptimizationObjectivePtr &opt) const
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 auto *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 auto *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)) != nullptr)
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 auto *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  auto 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  return false;
244  }
245 
246  bool valid = true;
247  const auto *si = static_cast<const SpaceInformation *>(si_.get());
248  double res = si->getPropagationStepSize();
249  base::State *next = si_->allocState();
250  for (unsigned int i = 0; valid && i < controls_.size(); ++i)
251  {
252  auto steps = (unsigned int)floor(0.5 + controlDurations_[i] / res);
253  if (!si->isValid(states_[i]) || si->propagateWhileValid(states_[i], controls_[i], steps, next) != steps ||
254  si->distance(next, states_[i + 1]) > std::numeric_limits<float>::epsilon())
255  valid = false;
256  }
257  si_->freeState(next);
258 
259  return valid;
260 }
261 
263 {
264  states_.push_back(si_->cloneState(state));
265 }
266 
267 void ompl::control::PathControl::append(const base::State *state, const Control *control, double duration)
268 {
269  const auto *si = static_cast<const SpaceInformation *>(si_.get());
270  states_.push_back(si->cloneState(state));
271  controls_.push_back(si->cloneControl(control));
272  controlDurations_.push_back(duration);
273 }
274 
276 {
277  freeMemory();
278  states_.resize(2);
279  controlDurations_.resize(1);
280  controls_.resize(1);
281 
282  const auto *si = static_cast<const SpaceInformation *>(si_.get());
283  states_[0] = si->allocState();
284  states_[1] = si->allocState();
285  controls_[0] = si->allocControl();
286 
287  base::StateSamplerPtr ss = si->allocStateSampler();
288  ss->sampleUniform(states_[0]);
289  ControlSamplerPtr cs = si->allocControlSampler();
290  cs->sample(controls_[0], states_[0]);
291  unsigned int steps = cs->sampleStepCount(si->getMinControlDuration(), si->getMaxControlDuration());
292  controlDurations_[0] = steps * si->getPropagationStepSize();
293  si->propagate(states_[0], controls_[0], steps, states_[1]);
294 }
295 
296 bool ompl::control::PathControl::randomValid(unsigned int attempts)
297 {
298  freeMemory();
299  states_.resize(2);
300  controlDurations_.resize(1);
301  controls_.resize(1);
302 
303  const auto *si = static_cast<const SpaceInformation *>(si_.get());
304  states_[0] = si->allocState();
305  states_[1] = si->allocState();
306  controls_[0] = si->allocControl();
307 
308  ControlSamplerPtr cs = si->allocControlSampler();
309  auto uvss(std::make_shared<base::UniformValidStateSampler>(si));
310  uvss->setNrAttempts(attempts);
311  bool ok = false;
312  for (unsigned int i = 0; i < attempts; ++i)
313  if (uvss->sample(states_[0]))
314  {
315  cs->sample(controls_[0], states_[0]);
316  unsigned int steps = cs->sampleStepCount(si->getMinControlDuration(), si->getMaxControlDuration());
317  controlDurations_[0] = steps * si->getPropagationStepSize();
318  if (si->propagateWhileValid(states_[0], controls_[0], steps, states_[1]) == steps)
319  {
320  ok = true;
321  break;
322  }
323  }
324 
325  if (!ok)
326  {
327  freeMemory();
328  states_.clear();
329  controls_.clear();
330  controlDurations_.clear();
331  }
332  return ok;
333 }
334 
336 {
337  for (auto &state : states_)
338  si_->freeState(state);
339  const auto *si = static_cast<const SpaceInformation *>(si_.get());
340  for (auto &control : controls_)
341  si->freeControl(control);
342 }
virtual bool isCompound() const
Check if the control space is compound.
Definition of an abstract control.
Definition: Control.h:111
A control space to allow the composition of control spaces.
Definition: ControlSpace.h:265
A space representing discrete controls; i.e. there are a small number of discrete controls the system...
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
Definition of an abstract state.
Definition: State.h:113
std::vector< Control * > controls_
The control applied at each state. This array contains one element less than the list of states.
Definition: PathControl.h:240
virtual double * getValueAddressAtIndex(Control *control, unsigned int index) const
Many controls contain a number of double values. This function provides a means to get the memory add...
bool randomValid(unsigned int attempts)
Set this path to a random valid segment. Sample attempts times for valid segments....
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,...
void random()
Set this path to a random segment.
virtual void copyToReals(std::vector< double > &reals, const State *source) const
Copy all the real values from a state source to the array reals using getValueAddressAtLocation()
Definition: StateSpace.cpp:329
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
void freeMemory()
Free the memory allocated by the path.
A control space representing the space of applicable controls.
Definition: ControlSpace.h:127
Definition of a geometric path.
Definition: PathGeometric.h:97
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 copyFrom(const PathControl &other)
Copy the content of a path to this one.
double length() const override
The path length (sum of control durations)
PathControl(const base::SpaceInformationPtr &si)
Constructor.
Definition: PathControl.cpp:80
SpaceInformationPtr si_
The space information this path is part of.
Definition: Path.h:187
T * as()
Cast this instance to a desired type.
Definition: ControlSpace.h:144
Definition of a compound control.
Definition: Control.h:148
base::Cost cost(const base::OptimizationObjectivePtr &opt) const override
Not yet implemented.
geometric::PathGeometric asGeometric() const
Convert this path into a geometric path (interpolation is performed and then states are copied)
Definition: PathControl.cpp:91
void print(std::ostream &out) const override
Print the path to a stream.
bool check() const override
Check if the path is valid.
A shared pointer wrapper for ompl::control::ControlSampler.
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...
Space information containing necessary information for planning with controls. setup() needs to be ca...
void interpolate()
Make the path such that all controls are applied for a single time step (computes intermediate states...
Definition of a control path.
Definition: PathControl.h:92
PathControl & operator=(const PathControl &other)
Assignment operator.
int value
The current control - an int in range [lowerBound, upperBound].
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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:244
The exception type for ompl.
Definition: Exception.h:78
std::vector< base::State * > states_
The list of states that make up the path.
Definition: PathControl.h:236