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
64 {
65 public:
66 // non-copyable
67 ControlSpace(const ControlSpace &) = delete;
68 ControlSpace &operator=(const ControlSpace &) = delete;
69
71 ControlSpace(base::StateSpacePtr stateSpace);
72
73 virtual ~ControlSpace();
74
76 template <class T>
77 T *as()
78 {
80 BOOST_CONCEPT_ASSERT((boost::Convertible<T *, ControlSpace *>));
81
82 return static_cast<T *>(this);
83 }
84
86 template <class T>
87 const T *as() const
88 {
90 BOOST_CONCEPT_ASSERT((boost::Convertible<T *, ControlSpace *>));
91
92 return static_cast<const T *>(this);
93 }
94
96 const std::string &getName() const;
97
99 void setName(const std::string &name);
100
104 int getType() const
105 {
106 return type_;
107 }
108
110 const base::StateSpacePtr &getStateSpace() const
111 {
112 return stateSpace_;
113 }
114
116 virtual unsigned int getDimension() const = 0;
117
119 virtual Control *allocControl() const = 0;
120
122 virtual void freeControl(Control *control) const = 0;
123
125 virtual void copyControl(Control *destination, const Control *source) const = 0;
126
128 virtual bool equalControls(const Control *control1, const Control *control2) const = 0;
129
131 virtual void nullControl(Control *control) const = 0;
132
135
142
145
148
155 virtual double *getValueAddressAtIndex(Control *control, unsigned int index) const;
156
158 virtual void printControl(const Control *control, std::ostream &out) const;
159
161 virtual void printSettings(std::ostream &out) const;
162
164 virtual void setup();
165
167 virtual unsigned int getSerializationLength() const;
168
170 virtual void serialize(void *serialization, const Control *ctrl) const;
171
173 virtual void deserialize(Control *ctrl, const void *serialization) const;
174
177 void computeSignature(std::vector<int> &signature) const;
178
180 virtual bool isCompound() const;
181
182 protected:
184 int type_;
185
187 base::StateSpacePtr stateSpace_;
188
191
192 private:
194 std::string name_;
195 };
196
199 {
200 public:
203
205 CompoundControlSpace(const base::StateSpacePtr &stateSpace)
206 : ControlSpace(stateSpace), componentCount_(0), locked_(false)
207 {
208 }
209
210 ~CompoundControlSpace() override = default;
211
213 template <class T>
214 T *as(const unsigned int index) const
215 {
217 BOOST_CONCEPT_ASSERT((boost::Convertible<T *, ControlSpace *>));
218
219 return static_cast<T *>(getSubspace(index).get());
220 }
221
223 virtual void addSubspace(const ControlSpacePtr &component);
224
226 unsigned int getSubspaceCount() const;
227
229 const ControlSpacePtr &getSubspace(unsigned int index) const;
230
232 const ControlSpacePtr &getSubspace(const std::string &name) const;
233
234 unsigned int getDimension() const override;
235
236 Control *allocControl() const override;
237
238 void freeControl(Control *control) const override;
239
240 void copyControl(Control *destination, const Control *source) const override;
241
242 bool equalControls(const Control *control1, const Control *control2) const override;
243
244 void nullControl(Control *control) const override;
245
247
248 double *getValueAddressAtIndex(Control *control, unsigned int index) const override;
249
250 void printControl(const Control *control, std::ostream &out = std::cout) const override;
251
252 void printSettings(std::ostream &out) const override;
253
254 void setup() override;
255
257 unsigned int getSerializationLength() const override;
258
260 void serialize(void *serialization, const Control *ctrl) const override;
261
263 void deserialize(Control *ctrl, const void *serialization) const override;
264
265 bool isCompound() const override;
266
272 void lock();
273
274 protected:
276 std::vector<ControlSpacePtr> components_;
277
279 unsigned int componentCount_;
280
283 };
284 }
285}
286
287#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.
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.
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
std::function< ControlSamplerPtr(const ControlSpace *)> ControlSamplerAllocator
Definition of a function that can allocate a control sampler.
Main namespace. Contains everything in this library.