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
const base::StateSpacePtr & getStateSpace() const
Return the state space this control space depends on.
Definition: ControlSpace.h:110
virtual bool isCompound() const
Check if the control space is compound.
unsigned int getSubspaceCount() const
Get the number of control spaces that make up the compound control space.
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation.
Definition of an abstract control.
Definition: Control.h:48
unsigned int componentCount_
The number of contained components.
Definition: ControlSpace.h:279
bool locked_
Flag indicating whether adding further components is allowed or not.
Definition: ControlSpace.h:282
A control space to allow the composition of control spaces.
Definition: ControlSpace.h:199
std::function< ControlSamplerPtr(const ControlSpace *)> ControlSamplerAllocator
Definition of a function that can allocate a control sampler.
bool isCompound() const override
Check if the control space is compound.
const std::string & getName() const
Get the name of the control space.
A shared pointer wrapper for ompl::control::ControlSpace.
const ControlSpacePtr & getSubspace(unsigned int index) const
Get a specific subspace from the compound control space.
void lock()
Lock this control space. This means no further control spaces can be added as components....
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...
unsigned int getDimension() const override
Get the dimension of this control space.
void setName(const std::string &name)
Set the name of the control space.
void setControlSamplerAllocator(const ControlSamplerAllocator &csa)
Set the sampler allocator to use.
void computeSignature(std::vector< int > &signature) const
Compute an array of ints that uniquely identifies the structure of the control space....
void freeControl(Control *control) const override
Free the memory of a control.
CompoundControlSpace(const base::StateSpacePtr &stateSpace)
Constructor. The corresponding state space needs to be specified.
Definition: ControlSpace.h:205
virtual Control * allocControl() const =0
Allocate memory for a control.
virtual ControlSamplerPtr allocControlSampler() const
Allocate an instance of the control sampler for this space. This sampler will be allocated with the s...
void serialize(void *serialization, const Control *ctrl) const override
Serializes the given control into the serialization buffer.
A control space representing the space of applicable controls.
Definition: ControlSpace.h:64
virtual unsigned int getDimension() const =0
Get the dimension of this control space.
ControlSamplerPtr allocDefaultControlSampler() const override
Allocate the default control sampler.
virtual void printSettings(std::ostream &out) const
Print the settings for this control space to a stream.
void printControl(const Control *control, std::ostream &out=std::cout) const override
Print a control to a stream.
int type_
A type assigned for this control space.
Definition: ControlSpace.h:184
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
T * as()
Cast this instance to a desired type.
Definition: ControlSpace.h:77
void printSettings(std::ostream &out) const override
Print the settings for this control space to a stream.
Definition of a compound control.
Definition: Control.h:85
Control * allocControl() const override
Allocate memory for a control.
void clearControlSamplerAllocator()
Clear the control sampler allocator (reset to default)
virtual void printControl(const Control *control, std::ostream &out) const
Print a control to a stream.
virtual void freeControl(Control *control) const =0
Free the memory of a control.
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation.
virtual unsigned int getSerializationLength() const
Returns the serialization size for a single control in this space.
void deserialize(Control *ctrl, const void *serialization) const override
Deserializes a control from the serialization buffer.
std::vector< ControlSpacePtr > components_
The component control spaces that make up the compound control space.
Definition: ControlSpace.h:276
virtual void addSubspace(const ControlSpacePtr &component)
Adds a control space as a component of the compound control space.
base::StateSpacePtr stateSpace_
The state space controls can be applied to.
Definition: ControlSpace.h:187
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.
A shared pointer wrapper for ompl::control::ControlSampler.
unsigned int getSerializationLength() const override
Returns the serialization size for a single control in this space.
virtual void deserialize(Control *ctrl, const void *serialization) const
Deserializes a control from the serialization buffer.
virtual bool equalControls(const Control *control1, const Control *control2) const =0
Check if two controls are the same.
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...
virtual void serialize(void *serialization, const Control *ctrl) const
Serializes the given control into the serialization buffer.
ControlSamplerAllocator csa_
An optional control sampler allocator.
Definition: ControlSpace.h:190
void copyControl(Control *destination, const Control *source) const override
Copy a control to another.
virtual ControlSamplerPtr allocDefaultControlSampler() const =0
Allocate the default control sampler.
const T * as() const
Cast this instance to a desired type.
Definition: ControlSpace.h:87
virtual void copyControl(Control *destination, const Control *source) const =0
Copy a control to another.
bool equalControls(const Control *control1, const Control *control2) const override
Check if two controls are the same.
T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: ControlSpace.h:214
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
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.