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"
55 for (
unsigned int i = 0; i < ccs->getSubspaceCount(); ++i)
56 num += getNumberOfDiscreteControls(ccs->getSubspace(i).get());
71 for (
unsigned int i = 0; i < ccs->getSubspaceCount(); ++i)
72 printDiscreteControls(out, ccs->getSubspace(i).get(),
83 throw Exception(
"Cannot create a path with controls from a space that does not support controls");
110 states_.resize(other.
states_.size());
111 controls_.resize(other.
controls_.size());
113 for (
unsigned int i = 0; i < states_.size(); ++i)
114 states_[i] = si_->cloneState(other.
states_[i]);
117 for (
unsigned int i = 0; i < controls_.size(); ++i)
118 controls_[i] = si->cloneControl(other.
controls_[i]);
125 OMPL_ERROR(
"Error: Cost computation is only implemented for paths of type PathGeometric.");
126 return opt->identityCost();
131 return std::accumulate(controlDurations_.begin(), controlDurations_.end(), 0.0);
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)
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;
147 out <<
"Arrive at state ";
148 si_->printState(states_[controls_.size()], out);
158 const ControlSpace *cspace(si->getControlSpace().get());
159 std::vector<double> reals;
162 std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out,
" "));
163 if (controls_.empty())
167 unsigned int n = 0, m = getNumberOfDiscreteControls(cs);
171 for (
unsigned int i = 0; i < n + m; ++i)
173 out <<
'0' << std::endl;
174 for (
unsigned int i = 0; i < controls_.size(); ++i)
177 std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out,
" "));
179 printDiscreteControls(out, cs, controls_[i]);
181 for (
unsigned int j = 0; j < n; ++j)
183 out << controlDurations_[i] << std::endl;
189 if (states_.size() <= controls_.size())
191 OMPL_ERROR(
"Interpolation not performed. Number of states in the path should be strictly greater than the "
192 "number of controls.");
197 std::vector<base::State *> newStates;
198 std::vector<Control *> newControls;
199 std::vector<double> newControlDurations;
201 double res = si->getPropagationStepSize();
202 for (
unsigned int i = 0; i < controls_.size(); ++i)
204 auto steps = (int)floor(0.5 + controlDurations_[i] / res);
208 newStates.push_back(states_[i]);
209 newControls.push_back(controls_[i]);
210 newControlDurations.push_back(controlDurations_[i]);
213 std::vector<base::State *> istates;
214 si->propagate(states_[i], controls_[i], steps, istates,
true);
216 if (!istates.empty())
218 si_->freeState(istates.back());
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)
227 newControls.push_back(si->cloneControl(controls_[i]));
228 newControlDurations.push_back(res);
231 newStates.push_back(states_[controls_.size()]);
232 states_.swap(newStates);
233 controls_.swap(newControls);
234 controlDurations_.swap(newControlDurations);
239 if (controls_.empty())
241 if (states_.size() == 1)
242 return si_->isValid(states_[0]);
248 double res = si->getPropagationStepSize();
250 for (
unsigned int i = 0; valid && i < controls_.size(); ++i)
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())
257 si_->freeState(next);
264 states_.push_back(si_->cloneState(state));
270 states_.push_back(si->cloneState(state));
271 controls_.push_back(si->cloneControl(control));
272 controlDurations_.push_back(duration);
279 controlDurations_.resize(1);
283 states_[0] = si->allocState();
284 states_[1] = si->allocState();
285 controls_[0] = si->allocControl();
287 base::StateSamplerPtr ss = si->allocStateSampler();
288 ss->sampleUniform(states_[0]);
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]);
300 controlDurations_.resize(1);
304 states_[0] = si->allocState();
305 states_[1] = si->allocState();
306 controls_[0] = si->allocControl();
309 auto uvss(std::make_shared<base::UniformValidStateSampler>(si));
310 uvss->setNrAttempts(attempts);
312 for (
unsigned int i = 0; i < attempts; ++i)
313 if (uvss->sample(states_[0]))
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)
330 controlDurations_.clear();
337 for (
auto &state : states_)
338 si_->freeState(state);
340 for (
auto &control : controls_)
341 si->freeControl(control);
The exception type for ompl.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
SpaceInformationPtr si_
The space information this path is part of.
Representation of a space in which planning can be performed. Topology specific sampling,...
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 of an abstract state.
A control space to allow the composition of control spaces.
Definition of a compound control.
A shared pointer wrapper for ompl::control::ControlSampler.
A control space representing the space of applicable controls.
virtual bool isCompound() const
Check if the control space is compound.
T * as()
Cast this instance to a desired type.
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...
Definition of an abstract control.
The definition of a discrete control.
int value
The current control - an int in range [lowerBound, upperBound].
A space representing discrete controls; i.e. there are a small number of discrete controls the system...
Definition of a control path.
void print(std::ostream &out) const override
Print the path to a stream.
bool check() const override
Check if the path is valid.
base::Cost cost(const base::OptimizationObjectivePtr &opt) const override
Not yet implemented.
bool randomValid(unsigned int attempts)
Set this path to a random valid segment. Sample attempts times for valid segments....
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...
void random()
Set this path to a random segment.
PathControl & operator=(const PathControl &other)
Assignment operator.
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)
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(const base::SpaceInformationPtr &si)
Constructor.
std::vector< double > controlDurations_
The duration of the control applied at each state. This array contains one element less than the list...
void freeMemory()
Free the memory allocated by the path.
std::vector< base::State * > states_
The list of states that make up the path.
std::vector< Control * > controls_
The control applied at each state. This array contains one element less than the list of states.
geometric::PathGeometric asGeometric() const
Convert this path into a geometric path (interpolation is performed and then states are copied)
void interpolate()
Make the path such that all controls are applied for a single time step (computes intermediate states...
Definition of a geometric path.
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.