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 
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 
137  virtual ControlSamplerPtr allocDefaultControlSampler() const = 0;
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 
285  bool locked_;
286  };
287  }
288 }
289 
290 #endif
const base::StateSpacePtr & getStateSpace() const
Return the state space this control space depends on.
Definition: ControlSpace.h:177
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:111
unsigned int componentCount_
The number of contained components.
Definition: ControlSpace.h:346
bool locked_
Flag indicating whether adding further components is allowed or not.
Definition: ControlSpace.h:349
A control space to allow the composition of control spaces.
Definition: ControlSpace.h:265
std::function< ControlSamplerPtr(const ControlSpace *)> ControlSamplerAllocator
Definition of a function that can allocate a control sampler.
ompl::control::CompoundControl ControlType
Define the type of control allocated by this control space.
Definition: ControlSpace.h:269
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:272
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:127
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:251
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:171
T * as()
Cast this instance to a desired type.
Definition: ControlSpace.h:144
void printSettings(std::ostream &out) const override
Print the settings for this control space to a stream.
ompl::control::Control ControlType
Define the type of control allocated by this control space.
Definition: ControlSpace.h:131
Definition of a compound control.
Definition: Control.h:148
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:343
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:254
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:257
void copyControl(Control *destination, const Control *source) const override
Copy a control to another.
virtual ControlSamplerPtr allocDefaultControlSampler() const =0
Allocate the default control sampler.
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.
Main namespace. Contains everything in this library.
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.