37 #include "ompl/control/PathControl.h"
38 #include "ompl/geometric/PathGeometric.h"
39 #include "ompl/base/samplers/UniformValidStateSampler.h"
40 #include "ompl/util/Exception.h"
47 if (!dynamic_cast<const SpaceInformation*>(
si_.get()))
48 throw Exception(
"Cannot create a path with controls from a space that does not support controls");
75 states_.resize(other.
states_.size());
78 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
79 states_[i] = si_->cloneState(other.
states_[i]);
82 for (
unsigned int i = 0 ; i < controls_.size() ; ++i)
90 return std::accumulate(controlDurations_.begin(), controlDurations_.end(), 0.0);
97 out <<
"Control path with " << states_.size() <<
" states" << std::endl;
98 for (
unsigned int i = 0 ; i < controls_.size() ; ++i)
101 si_->printState(states_[i], out);
102 out <<
" apply control ";
104 out <<
" for " << (int)floor(0.5 + controlDurations_[i]/res) <<
" steps" << std::endl;
106 out <<
"Arrive at state ";
107 si_->printState(states_[controls_.size()], out);
113 if (states_.size() <= controls_.size())
115 logError(
"Interpolation not performed. Number of states in the path should be strictly greater than the number of controls.");
120 std::vector<base::State*> newStates;
121 std::vector<Control*> newControls;
122 std::vector<double> newControlDurations;
125 for (
unsigned int i = 0 ; i < controls_.size() ; ++i)
127 int steps = (int)floor(0.5 + controlDurations_[i] / res);
131 newStates.push_back(states_[i]);
132 newControls.push_back(controls_[i]);
133 newControlDurations.push_back(controlDurations_[i]);
136 std::vector<base::State*> istates;
137 si->
propagate(states_[i], controls_[i], steps, istates,
true);
139 if (!istates.empty())
141 si_->freeState(istates.back());
144 newStates.push_back(states_[i]);
145 newStates.insert(newStates.end(), istates.begin(), istates.end());
146 newControls.push_back(controls_[i]);
147 newControlDurations.push_back(res);
148 for (
int j = 1 ; j < steps; ++j)
151 newControlDurations.push_back(res);
154 newStates.push_back(states_[controls_.size()]);
155 states_.swap(newStates);
156 controls_.swap(newControls);
157 controlDurations_.swap(newControlDurations);
162 if (controls_.empty())
164 if (states_.size() == 1)
165 return si_->isValid(states_[0]);
174 for (
unsigned int i = 0 ; valid && i < controls_.size() ; ++i)
176 unsigned int steps = (
unsigned int)floor(0.5 + controlDurations_[i] / res);
180 si_->freeState(dummy);
187 states_.push_back(si_->cloneState(state));
195 controlDurations_.push_back(duration);
202 controlDurations_.resize(1);
211 ss->sampleUniform(states_[0]);
213 cs->sample(controls_[0], states_[0]);
216 si->
propagate(states_[0], controls_[0], steps, states_[1]);
223 controlDurations_.resize(1);
235 for (
unsigned int i = 0 ; i < attempts ; ++i)
236 if (uvss->
sample(states_[0]))
238 cs->sample(controls_[0], states_[0]);
254 controlDurations_.clear();
261 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
262 si_->freeState(states_[i]);
264 for (
unsigned int i = 0 ; i < controls_.size() ; ++i)