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 
50 namespace 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 
141  virtual ControlSamplerPtr allocControlSampler() const;
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 
282  bool locked_;
283  };
284  }
285 }
286 
287 #endif
A control space to allow the composition of control spaces.
Definition: ControlSpace.h:199
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.
Definition: ControlSpace.h:282
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.
Definition: ControlSpace.h:276
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.
unsigned int componentCount_
The number of contained components.
Definition: ControlSpace.h:279
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.
T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: ControlSpace.h:214
CompoundControlSpace(const base::StateSpacePtr &stateSpace)
Constructor. The corresponding state space needs to be specified.
Definition: ControlSpace.h:205
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.
Definition: ControlSpace.h:64
const base::StateSpacePtr & getStateSpace() const
Return the state space this control space depends on.
Definition: ControlSpace.h:110
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation.
const T * as() const
Cast this instance to a desired type.
Definition: ControlSpace.h:87
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 ...
Definition: ControlSpace.h:104
virtual Control * allocControl() const =0
Allocate memory for a control.
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
virtual void freeControl(Control *control) const =0
Free the memory of a control.
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 ControlSamplerPtr allocDefaultControlSampler() const =0
Allocate the default control sampler.
ControlSamplerAllocator csa_
An optional control sampler allocator.
Definition: ControlSpace.h:190
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)
T * as()
Cast this instance to a desired type.
Definition: ControlSpace.h:77
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.
Definition: AppBase.h:22