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/util/Exception.h"
56 num += getNumberOfDiscreteControls(ccs->
getSubspace(i).get());
61 if (dynamic_cast<const ompl::control::DiscreteControlSpace*>(cs))
74 printDiscreteControls(out, ccs->
getSubspace(i).get(),
77 else if (dynamic_cast<const ompl::control::DiscreteControlSpace*>(cs))
84 if (!dynamic_cast<const SpaceInformation*>(
si_.get()))
85 throw Exception(
"Cannot create a path with controls from a space that does not support controls");
112 states_.resize(other.
states_.size());
113 controls_.resize(other.
controls_.size());
115 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
116 states_[i] = si_->cloneState(other.
states_[i]);
119 for (
unsigned int i = 0 ; i < controls_.size() ; ++i)
127 return std::accumulate(controlDurations_.begin(), controlDurations_.end(), 0.0);
134 out <<
"Control path with " << states_.size() <<
" states" << std::endl;
135 for (
unsigned int i = 0 ; i < controls_.size() ; ++i)
138 si_->printState(states_[i], out);
139 out <<
" apply control ";
141 out <<
" for " << (int)floor(0.5 + controlDurations_[i]/res) <<
" steps" << std::endl;
143 out <<
"Arrive at state ";
144 si_->printState(states_[controls_.size()], out);
154 const ControlSpace* cspace(si->getControlSpace().get());
155 std::vector<double> reals;
157 space->copyToReals(reals, states_[0]);
158 std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out,
" "));
159 if (controls_.empty())
163 unsigned int n = 0, m = getNumberOfDiscreteControls(cs);
165 while ((val = cspace->getValueAddressAtIndex(controls_[0], n)))
167 for (
unsigned int i = 0 ; i < n + m; ++i)
169 out <<
'0' << std::endl;
170 for (
unsigned int i = 0 ; i < controls_.size(); ++i)
172 space->copyToReals(reals, states_[i + 1]);
173 std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out,
" "));
175 printDiscreteControls(out, cs, controls_[i]);
177 for (
unsigned int j = 0; j < n; ++j)
178 out << *cspace->getValueAddressAtIndex(controls_[i], j) <<
' ';
179 out << controlDurations_[i] << std::endl;
185 if (states_.size() <= controls_.size())
187 OMPL_ERROR(
"Interpolation not performed. Number of states in the path should be strictly greater than the number of controls.");
192 std::vector<base::State*> newStates;
193 std::vector<Control*> newControls;
194 std::vector<double> newControlDurations;
197 for (
unsigned int i = 0 ; i < controls_.size() ; ++i)
199 int steps = (int)floor(0.5 + controlDurations_[i] / res);
203 newStates.push_back(states_[i]);
204 newControls.push_back(controls_[i]);
205 newControlDurations.push_back(controlDurations_[i]);
208 std::vector<base::State*> istates;
209 si->
propagate(states_[i], controls_[i], steps, istates,
true);
211 if (!istates.empty())
213 si_->freeState(istates.back());
216 newStates.push_back(states_[i]);
217 newStates.insert(newStates.end(), istates.begin(), istates.end());
218 newControls.push_back(controls_[i]);
219 newControlDurations.push_back(res);
220 for (
int j = 1 ; j < steps; ++j)
223 newControlDurations.push_back(res);
226 newStates.push_back(states_[controls_.size()]);
227 states_.swap(newStates);
228 controls_.swap(newControls);
229 controlDurations_.swap(newControlDurations);
234 if (controls_.empty())
236 if (states_.size() == 1)
237 return si_->isValid(states_[0]);
246 for (
unsigned int i = 0 ; valid && i < controls_.size() ; ++i)
248 unsigned int steps = (
unsigned int)floor(0.5 + controlDurations_[i] / res);
249 if (!si->
isValid(states_[i]) ||
251 si->
distance(next, states_[i + 1]) > std::numeric_limits<float>::epsilon())
254 si_->freeState(next);
261 states_.push_back(si_->cloneState(state));
269 controlDurations_.push_back(duration);
276 controlDurations_.resize(1);
285 ss->sampleUniform(states_[0]);
287 cs->sample(controls_[0], states_[0]);
290 si->
propagate(states_[0], controls_[0], steps, states_[1]);
297 controlDurations_.resize(1);
309 for (
unsigned int i = 0 ; i < attempts ; ++i)
310 if (uvss->
sample(states_[0]))
312 cs->sample(controls_[0], states_[0]);
328 controlDurations_.clear();
335 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
336 si_->freeState(states_[i]);
338 for (
unsigned int i = 0 ; i < controls_.size() ; ++i)
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 & operator=(const PathControl &other)
Assignment operator.
void freeMemory(void)
Free the memory allocated by the path.
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 boost shared pointer wrapper for ompl::base::StateSampler.
A boost shared pointer wrapper for ompl::control::ControlSampler.
const ControlSpacePtr & getSubspace(const unsigned int index) const
Get a specific subspace from the compound control space.
unsigned int getSubspaceCount(void) const
Get the number of control spaces that make up the compound control space.
The definition of a discrete control.
std::vector< base::State * > & getStates(void)
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
virtual double length(void) const
The path length (sum of control durations)
Definition of a control path.
virtual void print(std::ostream &out) const
Print the path to a stream.
SpaceInformationPtr si_
The space information this path is part of.
geometric::PathGeometric asGeometric(void) const
Convert this path into a geometric path (interpolation is performed and then states are copied) ...
int value
The current control - an int in range [lowerBound, upperBound].
Definition of a compound control.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
void random(void)
Set this path to a random segment.
bool randomValid(unsigned int attempts)
Set this path to a random valid segment. Sample attempts times for valid segments. Returns true on success.
void interpolate(void)
Make the path such that all controls are applied for a single time step (computes intermediate states...
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.
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...
std::vector< base::State * > states_
The list of states that make up the path.
const T * as(void) const
Cast this instance to a desired type.
A control space to allow the composition of control spaces.
PathControl(const base::SpaceInformationPtr &si)
Constructor.
virtual bool check(void) const
Check if the path is valid.
virtual bool isCompound(void) const
Check if the control space is compound.
Definition of a geometric path.
void copyFrom(const PathControl &other)
Copy the content of a path to this one.
T * as(void)
Cast this instance to a desired type.
std::vector< Control * > controls_
The control applied at each state. This array contains one element less than the list of states...
void setNrAttempts(unsigned int attempts)
Finding a valid sample usually requires performing multiple attempts. This call allows setting the nu...