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
87{
88 copyFrom(path);
89}
90
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
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]);
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
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 ControlSamplerPtr allocControlSampler() const
Allocate an instance of the control sampler for this space. This sampler will be allocated with the s...
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.
double getPropagationStepSize() const
Propagation is performed at integer multiples of a specified step size. This function returns the val...
void freeControl(Control *control) const
Free the memory of a control.
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
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition Control.h:45