Loading...
Searching...
No Matches
PathControl.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2010, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Ioan Sucan */
36
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"
43#include "ompl/util/Console.h"
44#include <numeric>
45#include <cmath>
46
47namespace
48{
49 unsigned int getNumberOfDiscreteControls(const ompl::control::ControlSpace *cs)
50 {
51 if (cs->isCompound())
52 {
53 const auto *ccs = cs->as<ompl::control::CompoundControlSpace>();
54 unsigned int num = 0;
55 for (unsigned int i = 0; i < ccs->getSubspaceCount(); ++i)
56 num += getNumberOfDiscreteControls(ccs->getSubspace(i).get());
57
58 return num;
59 }
60 if (dynamic_cast<const ompl::control::DiscreteControlSpace *>(cs) != nullptr)
61 return 1;
62 return 0;
63 }
64
65 void printDiscreteControls(std::ostream &out, const ompl::control::ControlSpace *cs,
67 {
68 if (cs->isCompound())
69 {
70 const auto *ccs = cs->as<ompl::control::CompoundControlSpace>();
71 for (unsigned int i = 0; i < ccs->getSubspaceCount(); ++i)
72 printDiscreteControls(out, ccs->getSubspace(i).get(),
74 }
75 else if (dynamic_cast<const ompl::control::DiscreteControlSpace *>(cs) != nullptr)
76 out << c->as<ompl::control::DiscreteControlSpace::ControlType>()->value << ' ';
77 }
78}
79
80ompl::control::PathControl::PathControl(const base::SpaceInformationPtr &si) : base::Path(si)
81{
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");
84}
85
86ompl::control::PathControl::PathControl(const PathControl &path) : base::Path(path.si_)
87{
88 copyFrom(path);
89}
90
92{
93 PathControl pc(*this);
94 pc.interpolate();
96 pg.getStates().swap(pc.states_);
97 return pg;
98}
99
101{
102 freeMemory();
103 si_ = other.si_;
104 copyFrom(other);
105 return *this;
106}
107
109{
110 states_.resize(other.states_.size());
111 controls_.resize(other.controls_.size());
112
113 for (unsigned int i = 0; i < states_.size(); ++i)
114 states_[i] = si_->cloneState(other.states_[i]);
115
116 const auto *si = static_cast<const SpaceInformation *>(si_.get());
117 for (unsigned int i = 0; i < controls_.size(); ++i)
118 controls_[i] = si->cloneControl(other.controls_[i]);
119
120 controlDurations_ = other.controlDurations_;
121}
122
123ompl::base::Cost ompl::control::PathControl::cost(const base::OptimizationObjectivePtr &opt) const
124{
125 OMPL_ERROR("Error: Cost computation is only implemented for paths of type PathGeometric.");
126 return opt->identityCost();
127}
128
130{
131 return std::accumulate(controlDurations_.begin(), controlDurations_.end(), 0.0);
132}
133
134void ompl::control::PathControl::print(std::ostream &out) const
135{
136 const auto *si = static_cast<const SpaceInformation *>(si_.get());
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)
140 {
141 out << "At state ";
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;
146 }
147 out << "Arrive at state ";
148 si_->printState(states_[controls_.size()], out);
149 out << std::endl;
150}
151
152void ompl::control::PathControl::printAsMatrix(std::ostream &out) const
153{
154 if (states_.empty())
155 return;
156 const base::StateSpace *space(si_->getStateSpace().get());
157 const auto *si = static_cast<const SpaceInformation *>(si_.get());
158 const ControlSpace *cspace(si->getControlSpace().get());
159 std::vector<double> reals;
160
161 space->copyToReals(reals, states_[0]);
162 std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out, " "));
163 if (controls_.empty())
164 return;
165
166 const ControlSpace *cs = static_cast<const SpaceInformation *>(si_.get())->getControlSpace().get();
167 unsigned int n = 0, m = getNumberOfDiscreteControls(cs);
168 double *val;
169 while ((val = cspace->getValueAddressAtIndex(controls_[0], n)) != nullptr)
170 ++n;
171 for (unsigned int i = 0; i < n + m; ++i)
172 out << "0 ";
173 out << '0' << std::endl;
174 for (unsigned int i = 0; i < controls_.size(); ++i)
175 {
176 space->copyToReals(reals, states_[i + 1]);
177 std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out, " "));
178 // print discrete controls
179 printDiscreteControls(out, cs, controls_[i]);
180 // print real-valued controls
181 for (unsigned int j = 0; j < n; ++j)
182 out << *cspace->getValueAddressAtIndex(controls_[i], j) << ' ';
183 out << controlDurations_[i] << std::endl;
184 }
185}
186
188{
189 if (states_.size() <= controls_.size())
190 {
191 OMPL_ERROR("Interpolation not performed. Number of states in the path should be strictly greater than the "
192 "number of controls.");
193 return;
194 }
195
196 const auto *si = static_cast<const SpaceInformation *>(si_.get());
197 std::vector<base::State *> newStates;
198 std::vector<Control *> newControls;
199 std::vector<double> newControlDurations;
200
201 double res = si->getPropagationStepSize();
202 for (unsigned int i = 0; i < controls_.size(); ++i)
203 {
204 auto steps = (int)floor(0.5 + controlDurations_[i] / res);
205 assert(steps >= 0);
206 if (steps <= 1)
207 {
208 newStates.push_back(states_[i]);
209 newControls.push_back(controls_[i]);
210 newControlDurations.push_back(controlDurations_[i]);
211 continue;
212 }
213 std::vector<base::State *> istates;
214 si->propagate(states_[i], controls_[i], steps, istates, true);
215 // last state is already in the non-interpolated path
216 if (!istates.empty())
217 {
218 si_->freeState(istates.back());
219 istates.pop_back();
220 }
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)
226 {
227 newControls.push_back(si->cloneControl(controls_[i]));
228 newControlDurations.push_back(res);
229 }
230 }
231 newStates.push_back(states_[controls_.size()]);
232 states_.swap(newStates);
233 controls_.swap(newControls);
234 controlDurations_.swap(newControlDurations);
235}
236
238{
239 if (controls_.empty())
240 {
241 if (states_.size() == 1)
242 return si_->isValid(states_[0]);
243 return false;
244 }
245
246 bool valid = true;
247 const auto *si = static_cast<const SpaceInformation *>(si_.get());
248 double res = si->getPropagationStepSize();
249 base::State *next = si_->allocState();
250 for (unsigned int i = 0; valid && i < controls_.size(); ++i)
251 {
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())
255 valid = false;
256 }
257 si_->freeState(next);
258
259 return valid;
260}
261
263{
264 states_.push_back(si_->cloneState(state));
265}
266
267void ompl::control::PathControl::append(const base::State *state, const Control *control, double duration)
268{
269 const auto *si = static_cast<const SpaceInformation *>(si_.get());
270 states_.push_back(si->cloneState(state));
271 controls_.push_back(si->cloneControl(control));
272 controlDurations_.push_back(duration);
273}
274
276{
277 freeMemory();
278 states_.resize(2);
279 controlDurations_.resize(1);
280 controls_.resize(1);
281
282 const auto *si = static_cast<const SpaceInformation *>(si_.get());
283 states_[0] = si->allocState();
284 states_[1] = si->allocState();
285 controls_[0] = si->allocControl();
286
287 base::StateSamplerPtr ss = si->allocStateSampler();
288 ss->sampleUniform(states_[0]);
289 ControlSamplerPtr cs = si->allocControlSampler();
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]);
294}
295
297{
298 freeMemory();
299 states_.resize(2);
300 controlDurations_.resize(1);
301 controls_.resize(1);
302
303 const auto *si = static_cast<const SpaceInformation *>(si_.get());
304 states_[0] = si->allocState();
305 states_[1] = si->allocState();
306 controls_[0] = si->allocControl();
307
308 ControlSamplerPtr cs = si->allocControlSampler();
309 auto uvss(std::make_shared<base::UniformValidStateSampler>(si));
310 uvss->setNrAttempts(attempts);
311 bool ok = false;
312 for (unsigned int i = 0; i < attempts; ++i)
313 if (uvss->sample(states_[0]))
314 {
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)
319 {
320 ok = true;
321 break;
322 }
323 }
324
325 if (!ok)
326 {
327 freeMemory();
328 states_.clear();
329 controls_.clear();
330 controlDurations_.clear();
331 }
332 return ok;
333}
334
336{
337 for (auto &state : states_)
338 si_->freeState(state);
339 const auto *si = static_cast<const SpaceInformation *>(si_.get());
340 for (auto &control : controls_)
341 si->freeControl(control);
342}
The exception type for ompl.
Definition Exception.h:47
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
SpaceInformationPtr si_
The space information this path is part of.
Definition Path.h:123
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition StateSpace.h:71
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.
Definition State.h:50
A control space to allow the composition of control spaces.
Definition of a compound control.
Definition Control.h:85
Control ** components
The components that make up a compound control.
Definition Control.h:118
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.
Definition Control.h:48
A space representing discrete controls; i.e. there are a small number of discrete controls the system...
Definition of a control path.
Definition PathControl.h:61
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...
Space information containing necessary information for planning with controls. setup() needs to be ca...
const ControlSpacePtr & getControlSpace() const
Get the control space.
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.
Definition Console.h:64