37 #include "ompl/control/PathControl.h"
38 #include "ompl/geometric/PathGeometric.h"
39 #include "ompl/base/samplers/UniformValidStateSampler.h"
40 #include "ompl/base/OptimizationObjective.h"
41 #include "ompl/util/Exception.h"
48 if (!dynamic_cast<const SpaceInformation*>(
si_.get()))
49 throw Exception(
"Cannot create a path with controls from a space that does not support controls");
76 states_.resize(other.
states_.size());
79 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
80 states_[i] = si_->cloneState(other.
states_[i]);
83 for (
unsigned int i = 0 ; i < controls_.size() ; ++i)
91 return std::accumulate(controlDurations_.begin(), controlDurations_.end(), 0.0);
97 for (
unsigned int i = 1 ; i < states_.size() ; ++i)
108 out <<
"Control path with " << states_.size() <<
" states" << std::endl;
109 for (
unsigned int i = 0 ; i < controls_.size() ; ++i)
112 si_->printState(states_[i], out);
113 out <<
" apply control ";
115 out <<
" for " << (int)floor(0.5 + controlDurations_[i]/res) <<
" steps" << std::endl;
117 out <<
"Arrive at state ";
118 si_->printState(states_[controls_.size()], out);
124 if (states_.size() <= controls_.size())
126 OMPL_ERROR(
"Interpolation not performed. Number of states in the path should be strictly greater than the number of controls.");
131 std::vector<base::State*> newStates;
132 std::vector<Control*> newControls;
133 std::vector<double> newControlDurations;
136 for (
unsigned int i = 0 ; i < controls_.size() ; ++i)
138 int steps = (int)floor(0.5 + controlDurations_[i] / res);
142 newStates.push_back(states_[i]);
143 newControls.push_back(controls_[i]);
144 newControlDurations.push_back(controlDurations_[i]);
147 std::vector<base::State*> istates;
148 si->
propagate(states_[i], controls_[i], steps, istates,
true);
150 if (!istates.empty())
152 si_->freeState(istates.back());
155 newStates.push_back(states_[i]);
156 newStates.insert(newStates.end(), istates.begin(), istates.end());
157 newControls.push_back(controls_[i]);
158 newControlDurations.push_back(res);
159 for (
int j = 1 ; j < steps; ++j)
162 newControlDurations.push_back(res);
165 newStates.push_back(states_[controls_.size()]);
166 states_.swap(newStates);
167 controls_.swap(newControls);
168 controlDurations_.swap(newControlDurations);
173 if (controls_.empty())
175 if (states_.size() == 1)
176 return si_->isValid(states_[0]);
185 for (
unsigned int i = 0 ; valid && i < controls_.size() ; ++i)
187 unsigned int steps = (
unsigned int)floor(0.5 + controlDurations_[i] / res);
191 si_->freeState(dummy);
198 states_.push_back(si_->cloneState(state));
206 controlDurations_.push_back(duration);
213 controlDurations_.resize(1);
222 ss->sampleUniform(states_[0]);
224 cs->sample(controls_[0], states_[0]);
227 si->
propagate(states_[0], controls_[0], steps, states_[1]);
234 controlDurations_.resize(1);
246 for (
unsigned int i = 0 ; i < attempts ; ++i)
247 if (uvss->
sample(states_[0]))
249 cs->sample(controls_[0], states_[0]);
265 controlDurations_.clear();
272 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
273 si_->freeState(states_[i]);
275 for (
unsigned int i = 0 ; i < controls_.size() ; ++i)