ControlSpace.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 <utility>
38
39#include "ompl/control/ControlSpace.h"
40#include "ompl/util/Exception.h"
41
43namespace ompl
44{
45 namespace control
46 {
47 static void computeControlSpaceSignatureHelper(const ControlSpace *space, std::vector<int> &signature)
48 {
49 signature.push_back(space->getType());
50 signature.push_back(space->getDimension());
51
52 if (space->isCompound())
53 {
54 unsigned int c = space->as<CompoundControlSpace>()->getSubspaceCount();
55 for (unsigned int i = 0; i < c; ++i)
56 computeControlSpaceSignatureHelper(space->as<CompoundControlSpace>()->getSubspace(i).get(),
57 signature);
58 }
59 }
60 }
61}
63
64ompl::control::ControlSpace::ControlSpace(base::StateSpacePtr stateSpace) : stateSpace_(std::move(stateSpace))
65{
66 name_ = "Control[" + stateSpace_->getName() + "]";
68}
69
70ompl::control::ControlSpace::~ControlSpace() = default;
71
72const std::string &ompl::control::ControlSpace::getName() const
73{
74 return name_;
75}
76
77void ompl::control::ControlSpace::setName(const std::string &name)
78{
79 name_ = name;
80}
81
83{
84}
85
87{
88 if (csa_)
89 return csa_(this);
90 return allocDefaultControlSampler();
91}
92
94{
95 csa_ = csa;
96}
97
99{
101}
102
103double *ompl::control::ControlSpace::getValueAddressAtIndex(Control * /*control*/, const unsigned int /*index*/) const
104{
105 return nullptr;
106}
107
108void ompl::control::ControlSpace::printControl(const Control *control, std::ostream &out) const
109{
110 out << "Control instance: " << control << std::endl;
111}
112
114{
115 out << "ControlSpace '" << getName() << "' instance: " << this << std::endl;
116}
117
119{
120 return 0;
121}
122
123void ompl::control::ControlSpace::serialize(void * /*serialization*/, const Control * /*ctrl*/) const
124{
125}
126
127void ompl::control::ControlSpace::deserialize(Control * /*ctrl*/, const void * /*serialization*/) const
128{
129}
130
131void ompl::control::ControlSpace::computeSignature(std::vector<int> &signature) const
132{
133 signature.clear();
134 computeControlSpaceSignatureHelper(this, signature);
135 signature.insert(signature.begin(), signature.size());
136}
137
139{
140 return false;
141}
142
144{
145 if (locked_)
146 throw Exception("This control space is locked. No further components can be added");
147
148 components_.push_back(component);
149 componentCount_ = components_.size();
150}
151
153{
154 return componentCount_;
155}
156
158{
159 if (componentCount_ > index)
160 return components_[index];
161 throw Exception("Subspace index does not exist");
162}
163
165{
166 for (const auto &component : components_)
167 if (component->getName() == name)
168 return component;
169 throw Exception("Subspace " + name + " does not exist");
170}
171
173{
174 unsigned int dim = 0;
175 for (const auto &component : components_)
176 dim += component->getDimension();
177 return dim;
178}
179
181{
182 auto *control = new CompoundControl();
183 control->components = new Control *[componentCount_];
184 for (unsigned int i = 0; i < componentCount_; ++i)
185 control->components[i] = components_[i]->allocControl();
186 return static_cast<Control *>(control);
187}
188
190{
191 auto *ccontrol = static_cast<CompoundControl *>(control);
192 for (unsigned int i = 0; i < componentCount_; ++i)
193 components_[i]->freeControl(ccontrol->components[i]);
194 delete[] ccontrol->components;
195 delete ccontrol;
196}
197
199{
200 auto *cdest = static_cast<CompoundControl *>(destination);
201 const auto *csrc = static_cast<const CompoundControl *>(source);
202 for (unsigned int i = 0; i < componentCount_; ++i)
203 components_[i]->copyControl(cdest->components[i], csrc->components[i]);
204}
205
206bool ompl::control::CompoundControlSpace::equalControls(const Control *control1, const Control *control2) const
207{
208 const auto *ccontrol1 = static_cast<const CompoundControl *>(control1);
209 const auto *ccontrol2 = static_cast<const CompoundControl *>(control2);
210 for (unsigned int i = 0; i < componentCount_; ++i)
211 if (!components_[i]->equalControls(ccontrol1->components[i], ccontrol2->components[i]))
212 return false;
213 return true;
214}
215
217{
218 auto *ccontrol = static_cast<CompoundControl *>(control);
219 for (unsigned int i = 0; i < componentCount_; ++i)
220 components_[i]->nullControl(ccontrol->components[i]);
221}
222
224{
225 auto ss(std::make_shared<CompoundControlSampler>(this));
226 for (const auto &component : components_)
227 ss->addSampler(component->allocControlSampler());
228 return ss;
229}
230
232{
233 locked_ = true;
234}
235
236double *ompl::control::CompoundControlSpace::getValueAddressAtIndex(Control *control, const unsigned int index) const
237{
238 auto *ccontrol = static_cast<CompoundControl *>(control);
239 unsigned int idx = 0;
240
241 for (unsigned int i = 0; i < componentCount_; ++i)
242 for (unsigned int j = 0; j <= index; ++j)
243 {
244 double *va = components_[i]->getValueAddressAtIndex(ccontrol->components[i], j);
245 if (va != nullptr)
246 {
247 if (idx == index)
248 return va;
249 idx++;
250 }
251 else
252 break;
253 }
254 return nullptr;
255}
256
257void ompl::control::CompoundControlSpace::printControl(const Control *control, std::ostream &out) const
258{
259 out << "Compound control [" << std::endl;
260 const auto *ccontrol = static_cast<const CompoundControl *>(control);
261 for (unsigned int i = 0; i < componentCount_; ++i)
262 components_[i]->printControl(ccontrol->components[i], out);
263 out << "]" << std::endl;
264}
265
267{
268 out << "Compound control space '" << getName() << "' [" << std::endl;
269 for (unsigned int i = 0; i < componentCount_; ++i)
270 components_[i]->printSettings(out);
271 out << "]" << std::endl;
272}
273
275{
276 for (const auto &component : components_)
277 component->setup();
279}
280
282{
283 unsigned int l = 0;
284 for (const auto &component : components_)
285 l += component->getSerializationLength();
286 return l;
287}
288
289void ompl::control::CompoundControlSpace::serialize(void *serialization, const Control *ctrl) const
290{
291 const auto *compctrl = static_cast<const CompoundControl *>(ctrl);
292 unsigned int l = 0;
293 for (unsigned int i = 0; i < componentCount_; ++i)
294 {
295 components_[i]->serialize(reinterpret_cast<char *>(serialization) + l, compctrl->components[i]);
296 l += components_[i]->getSerializationLength();
297 }
298}
299
300void ompl::control::CompoundControlSpace::deserialize(Control *ctrl, const void *serialization) const
301{
302 auto *compctrl = static_cast<CompoundControl *>(ctrl);
303 unsigned int l = 0;
304 for (unsigned int i = 0; i < componentCount_; ++i)
305 {
306 components_[i]->deserialize(compctrl->components[i], reinterpret_cast<const char *>(serialization) + l);
307 l += components_[i]->getSerializationLength();
308 }
309}
310
312{
313 return true;
314}
The exception type for ompl.
Definition: Exception.h:47
unsigned int getSubspaceCount() const
Get the number of control spaces that make up the compound control space.
void lock()
Lock this control space. This means no further control spaces can be added as components....
double * getValueAddressAtIndex(Control *control, unsigned int index) const override
Many controls contain a number of double values. This function provides a means to get the memory add...
bool equalControls(const Control *control1, const Control *control2) const override
Check if two controls are the same.
void copyControl(Control *destination, const Control *source) const override
Copy a control to another.
unsigned int getSerializationLength() const override
Returns the serialization size for a single control in this space.
ControlSamplerPtr allocDefaultControlSampler() const override
Allocate the default control sampler.
void printSettings(std::ostream &out) const override
Print the settings for this control space to a stream.
void nullControl(Control *control) const override
Make the control have no effect if it were to be applied to a state for any amount of time.
unsigned int getDimension() const override
Get the dimension of this control space.
void serialize(void *serialization, const Control *ctrl) const override
Serializes the given control into the serialization buffer.
bool isCompound() const override
Check if the control space is compound.
Control * allocControl() const override
Allocate memory for a control.
void freeControl(Control *control) const override
Free the memory of a control.
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation.
virtual void addSubspace(const ControlSpacePtr &component)
Adds a control space as a component of the compound control space.
void deserialize(Control *ctrl, const void *serialization) const override
Deserializes a control from the serialization buffer.
void printControl(const Control *control, std::ostream &out=std::cout) const override
Print a control to a stream.
const ControlSpacePtr & getSubspace(unsigned int index) const
Get a specific subspace from the compound control space.
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 shared pointer wrapper for ompl::control::ControlSpace.
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation.
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.
virtual void printSettings(std::ostream &out) const
Print the settings for this control space to a stream.
virtual void serialize(void *serialization, const Control *ctrl) const
Serializes the given control into the serialization buffer.
void computeSignature(std::vector< int > &signature) const
Compute an array of ints that uniquely identifies the structure of the control space....
int type_
A type assigned for this control space.
Definition: ControlSpace.h:184
base::StateSpacePtr stateSpace_
The state space controls can be applied to.
Definition: ControlSpace.h:187
const std::string & getName() const
Get the name of the control space.
virtual void printControl(const Control *control, std::ostream &out) const
Print a control to a stream.
void clearControlSamplerAllocator()
Clear the control sampler allocator (reset to default)
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...
virtual void deserialize(Control *ctrl, const void *serialization) const
Deserializes a control from the serialization buffer.
void setName(const std::string &name)
Set the name of the control space.
virtual unsigned int getSerializationLength() const
Returns the serialization size for a single control in this space.
void setControlSamplerAllocator(const ControlSamplerAllocator &csa)
Set the sampler allocator to use.
Definition of an abstract control.
Definition: Control.h:48
@ CONTROL_SPACE_UNKNOWN
Unset type; this is the default type.
std::function< ControlSamplerPtr(const ControlSpace *)> ControlSamplerAllocator
Definition of a function that can allocate a control sampler.
Main namespace. Contains everything in this library.
STL namespace.