Loading...
Searching...
No Matches
ControlSpace.h
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#ifndef OMPL_CONTROL_CONTROL_SPACE_
38#define OMPL_CONTROL_CONTROL_SPACE_
39
40#include "ompl/base/StateSpace.h"
41#include "ompl/control/Control.h"
42#include "ompl/control/ControlSampler.h"
43#include "ompl/control/ControlSpaceTypes.h"
44#include "ompl/util/Console.h"
45#include "ompl/util/ClassForward.h"
46#include <boost/concept_check.hpp>
47#include <iostream>
48#include <vector>
49
50namespace ompl
51{
52 namespace control
53 {
55
56 OMPL_CLASS_FORWARD(ControlSpace);
58
61
63 class ControlSpace
64 {
65 public:
68
69 // non-copyable
70 ControlSpace(const ControlSpace &) = delete;
71 ControlSpace &operator=(const ControlSpace &) = delete;
72
74 ControlSpace(base::StateSpacePtr stateSpace);
75
76 virtual ~ControlSpace();
77
79 template <class T>
80 T *as()
81 {
83 BOOST_CONCEPT_ASSERT((boost::Convertible<T *, ControlSpace *>));
84
85 return static_cast<T *>(this);
86 }
87
89 template <class T>
90 const T *as() const
91 {
93 BOOST_CONCEPT_ASSERT((boost::Convertible<T *, ControlSpace *>));
94
95 return static_cast<const T *>(this);
96 }
97
99 const std::string &getName() const;
100
102 void setName(const std::string &name);
103
107 int getType() const
108 {
109 return type_;
110 }
111
113 const base::StateSpacePtr &getStateSpace() const
114 {
115 return stateSpace_;
116 }
117
119 virtual unsigned int getDimension() const = 0;
120
122 virtual Control *allocControl() const = 0;
123
125 virtual void freeControl(Control *control) const = 0;
126
128 virtual void copyControl(Control *destination, const Control *source) const = 0;
129
131 virtual bool equalControls(const Control *control1, const Control *control2) const = 0;
132
134 virtual void nullControl(Control *control) const = 0;
135
138
145
148
151
158 virtual double *getValueAddressAtIndex(Control *control, unsigned int index) const;
159
161 virtual void printControl(const Control *control, std::ostream &out) const;
162
164 virtual void printSettings(std::ostream &out) const;
165
167 virtual void setup();
168
170 virtual unsigned int getSerializationLength() const;
171
173 virtual void serialize(void *serialization, const Control *ctrl) const;
174
176 virtual void deserialize(Control *ctrl, const void *serialization) const;
177
180 void computeSignature(std::vector<int> &signature) const;
181
183 virtual bool isCompound() const;
184
185 protected:
187 int type_;
188
190 base::StateSpacePtr stateSpace_;
191
194
195 private:
197 std::string name_;
198 };
199
201 class CompoundControlSpace : public ControlSpace
202 {
203 public:
206
208 CompoundControlSpace(const base::StateSpacePtr &stateSpace)
209 : ControlSpace(stateSpace), componentCount_(0), locked_(false)
210 {
211 }
212
213 ~CompoundControlSpace() override = default;
214
216 template <class T>
217 T *as(const unsigned int index) const
218 {
220 BOOST_CONCEPT_ASSERT((boost::Convertible<T *, ControlSpace *>));
221
222 return static_cast<T *>(getSubspace(index).get());
223 }
224
226 virtual void addSubspace(const ControlSpacePtr &component);
227
229 unsigned int getSubspaceCount() const;
230
232 const ControlSpacePtr &getSubspace(unsigned int index) const;
233
235 const ControlSpacePtr &getSubspace(const std::string &name) const;
236
237 unsigned int getDimension() const override;
238
239 Control *allocControl() const override;
240
241 void freeControl(Control *control) const override;
242
243 void copyControl(Control *destination, const Control *source) const override;
244
245 bool equalControls(const Control *control1, const Control *control2) const override;
246
247 void nullControl(Control *control) const override;
248
250
251 double *getValueAddressAtIndex(Control *control, unsigned int index) const override;
252
253 void printControl(const Control *control, std::ostream &out = std::cout) const override;
254
255 void printSettings(std::ostream &out) const override;
256
257 void setup() override;
258
260 unsigned int getSerializationLength() const override;
261
263 void serialize(void *serialization, const Control *ctrl) const override;
264
266 void deserialize(Control *ctrl, const void *serialization) const override;
267
268 bool isCompound() const override;
269
275 void lock();
276
277 protected:
279 std::vector<ControlSpacePtr> components_;
280
282 unsigned int componentCount_;
283
286 };
287 }
288}
289
290#endif
A control space to allow the composition of control spaces.
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....
bool locked_
Flag indicating whether adding further components is allowed or not.
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.
std::vector< ControlSpacePtr > components_
The component control spaces that make up the compound control 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.
T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
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.
unsigned int componentCount_
The number of contained components.
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.
ompl::control::CompoundControl ControlType
Define the type of control allocated by this control space.
CompoundControlSpace(const base::StateSpacePtr &stateSpace)
Constructor. The corresponding state space needs to be specified.
Definition of a compound control.
Definition Control.h:85
A shared pointer wrapper for ompl::control::ControlSampler.
A shared pointer wrapper for ompl::control::ControlSpace.
A control space representing the space of applicable controls.
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation.
const base::StateSpacePtr & getStateSpace() const
Return the state space this control space depends on.
const T * as() const
Cast this instance to a desired type.
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.
int getType() const
Get the type of the control space. The type can be used to verify whether two space instances are of ...
void computeSignature(std::vector< int > &signature) const
Compute an array of ints that uniquely identifies the structure of the control space....
virtual Control * allocControl() const =0
Allocate memory for a control.
int type_
A type assigned for this control space.
virtual void freeControl(Control *control) const =0
Free the memory of a control.
base::StateSpacePtr stateSpace_
The state space controls can be applied to.
const std::string & getName() const
Get the name of the control space.
virtual ControlSamplerPtr allocDefaultControlSampler() const =0
Allocate the default control sampler.
T * as()
Cast this instance to a desired type.
ControlSamplerAllocator csa_
An optional control sampler allocator.
virtual void nullControl(Control *control) const =0
Make the control have no effect if it were to be applied to a state for any amount of time.
virtual void printControl(const Control *control, std::ostream &out) const
Print a control to a stream.
virtual unsigned int getDimension() const =0
Get the dimension of this control space.
ompl::control::Control ControlType
Define the type of control allocated by this control space.
virtual bool equalControls(const Control *control1, const Control *control2) const =0
Check if two controls are the same.
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 void copyControl(Control *destination, const Control *source) const =0
Copy a control to another.
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
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition Control.h:45
std::function< ControlSamplerPtr(const ControlSpace *)> ControlSamplerAllocator
Definition of a function that can allocate a control sampler.
Main namespace. Contains everything in this library.