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());
60 if (dynamic_cast<const ompl::control::DiscreteControlSpace *>(cs) !=
nullptr)
71 for (
unsigned int i = 0; i < ccs->getSubspaceCount(); ++i)
72 printDiscreteControls(out, ccs->getSubspace(i).get(),
75 else if (dynamic_cast<const ompl::control::DiscreteControlSpace *>(cs) !=
nullptr)
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");
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;
161 space->copyToReals(reals, states_[0]);
162 std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out,
" "));
163 if (controls_.empty())
167 unsigned int n = 0, m = getNumberOfDiscreteControls(cs);
169 while ((val = cspace->getValueAddressAtIndex(controls_[0], n)) !=
nullptr)
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)
176 space->copyToReals(reals, states_[i + 1]);
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)
182 out << *cspace->getValueAddressAtIndex(controls_[i], 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();
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);
PathControl & operator=(const PathControl &other)
Assignment operator.
const T * as() const
Cast this instance to a desired type.
void print(std::ostream &out) const override
Print the path to a stream.
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.
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...
geometric::PathGeometric asGeometric() const
Convert this path into a geometric path (interpolation is performed and then states are copied) ...
std::vector< Control * > controls_
The control applied at each state. This array contains one element less than the list of states...
The definition of a discrete control.
Definition of a control path.
SpaceInformationPtr si_
The space information this path is part of.
int value
The current control - an int in range [lowerBound, upperBound].
T * as()
Cast this instance to a desired type.
Definition of a compound control.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
bool randomValid(unsigned int attempts)
Set this path to a random valid segment. Sample attempts times for valid segments. Returns true on success.
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.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
void interpolate()
Make the path such that all controls are applied for a single time step (computes intermediate states...
Definition of an abstract state.
The exception type for ompl.
std::vector< double > controlDurations_
The duration of the control applied at each state. This array contains one element less than the list...
A shared pointer wrapper for ompl::base::OptimizationObjective.
base::Cost cost(const base::OptimizationObjectivePtr &opt) const override
Not yet implemented.
A control space to allow the composition of control spaces.
PathControl(const base::SpaceInformationPtr &si)
Constructor.
double length() const override
The path length (sum of control durations)
Definition of a geometric path.
void copyFrom(const PathControl &other)
Copy the content of a path to this one.
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.
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< base::State * > & getStates()
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...