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 
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 
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 
134  virtual ControlSamplerPtr allocDefaultControlSampler() const = 0;
135 
141  virtual ControlSamplerPtr allocControlSampler() const;
142 
145 
148 
155  virtual double *getValueAddressAtIndex(Control *control, const 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 
188 
191 
192  private:
194  std::string name_;
195  };
196 
199  {
200  public:
203 
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(const 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, const 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 T * as() const
Cast this instance to a desired type.
Definition: ControlSpace.h:87
virtual Control * allocControl() const =0
Allocate memory for a control.
virtual void deserialize(Control *ctrl, const void *serialization) const
Deserializes a control from the serialization buffer.
base::StateSpacePtr stateSpace_
The state space controls can be applied to.
Definition: ControlSpace.h:187
virtual void freeControl(Control *control) const =0
Free the memory of a control.
virtual void printControl(const Control *control, std::ostream &out) const
Print a control to a stream.
bool locked_
Flag indicating whether adding further components is allowed or not.
Definition: ControlSpace.h:282
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 ControlSamplerPtr allocDefaultControlSampler() const =0
Allocate the default control sampler.
Definition of an abstract control.
Definition: Control.h:47
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::control::ControlSampler.
virtual void copyControl(Control *destination, const Control *source) const =0
Copy a control to another.
void setName(const std::string &name)
Set the name of the control space.
void clearControlSamplerAllocator()
Clear the control sampler allocator (reset to default)
CompoundControlSpace(const base::StateSpacePtr &stateSpace)
Constructor. The corresponding state space needs to be specified.
Definition: ControlSpace.h:205
virtual void printSettings(std::ostream &out) const
Print the settings for this control space to a stream.
std::vector< ControlSpacePtr > components_
The component control spaces that make up the compound control space.
Definition: ControlSpace.h:276
A shared pointer wrapper for ompl::control::ControlSpace.
T * as()
Cast this instance to a desired type.
Definition: ControlSpace.h:77
virtual ControlSamplerPtr allocControlSampler() const
Allocate an instance of the control sampler for this space. This sampler will be allocated with the s...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void computeSignature(std::vector< int > &signature) const
Compute an array of ints that uniquely identifies the structure of the control space. The first element of the signature is the number of integers that follow.
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation.
Definition of a compound control.
Definition: Control.h:84
ompl::control::CompoundControl ControlType
Define the type of control allocated by this control space.
Definition: ControlSpace.h:202
int type_
A type assigned for this control space.
Definition: ControlSpace.h:184
const base::StateSpacePtr & getStateSpace() const
Return the state space this control space depends on.
Definition: ControlSpace.h:110
virtual unsigned int getDimension() const =0
Get the dimension of this control space.
A control space representing the space of applicable controls.
Definition: ControlSpace.h:63
virtual double * getValueAddressAtIndex(Control *control, const unsigned int index) const
Many controls contain a number of double values. This function provides a means to get the memory add...
virtual unsigned int getSerializationLength() const
Returns the serialization size for a single control in this space.
T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: ControlSpace.h:214
unsigned int componentCount_
The number of contained components.
Definition: ControlSpace.h:279
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...
void setControlSamplerAllocator(const ControlSamplerAllocator &csa)
Set the sampler allocator to use.
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
A control space to allow the composition of control spaces.
Definition: ControlSpace.h:198
virtual bool isCompound() const
Check if the control space is compound.
const std::string & getName() const
Get the name of the control space.
std::function< ControlSamplerPtr(const ControlSpace *)> ControlSamplerAllocator
Definition of a function that can allocate a control sampler.
virtual bool equalControls(const Control *control1, const Control *control2) const =0
Check if two controls are the same.