ControlSpace.cpp
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 #include "ompl/control/ControlSpace.h"
38 #include "ompl/util/Exception.h"
39 
41 namespace ompl
42 {
43  namespace control
44  {
45  static void computeControlSpaceSignatureHelper(const ControlSpace *space, std::vector<int> &signature)
46  {
47  signature.push_back(space->getType());
48  signature.push_back(space->getDimension());
49 
50  if (space->isCompound())
51  {
52  unsigned int c = space->as<CompoundControlSpace>()->getSubspaceCount();
53  for (unsigned int i = 0 ; i < c ; ++i)
54  computeControlSpaceSignatureHelper(space->as<CompoundControlSpace>()->getSubspace(i).get(), signature);
55  }
56  }
57  }
58 }
60 
61 ompl::control::ControlSpace::ControlSpace(const base::StateSpacePtr &stateSpace) : stateSpace_(stateSpace)
62 {
63  name_ = "Control[" + stateSpace_->getName() + "]";
65 }
66 
67 ompl::control::ControlSpace::~ControlSpace()
68 {
69 }
70 
71 const std::string& ompl::control::ControlSpace::getName() const
72 {
73  return name_;
74 }
75 
76 void ompl::control::ControlSpace::setName(const std::string &name)
77 {
78  name_ = name;
79 }
80 
82 {
83 }
84 
86 {
87  if (csa_)
88  return csa_(this);
89  else
91 }
92 
94 {
95  csa_ = csa;
96 }
97 
99 {
101 }
102 
103 double* ompl::control::ControlSpace::getValueAddressAtIndex(Control* /*control*/, const unsigned int /*index*/) const
104 {
105  return NULL;
106 }
107 
108 void ompl::control::ControlSpace::printControl(const Control *control, std::ostream &out) const
109 {
110  out << "Control instance: " << control << std::endl;
111 }
112 
113 void ompl::control::ControlSpace::printSettings(std::ostream &out) const
114 {
115  out << "ControlSpace '" << getName() << "' instance: " << this << std::endl;
116 }
117 
119 {
120  return 0;
121 }
122 
123 void ompl::control::ControlSpace::serialize(void* /*serialization*/, const Control* /*ctrl*/) const
124 {
125 }
126 
127 void ompl::control::ControlSpace::deserialize(Control* /*ctrl*/, const void* /*serialization*/) const
128 {
129 }
130 
131 void ompl::control::ControlSpace::computeSignature(std::vector<int> &signature) const
132 {
133  signature.clear();
134  computeControlSpaceSignatureHelper(this, signature);
135  signature.insert(signature.begin(), signature.size());
136 }
137 
139 {
140  return false;
141 }
142 
144 {
145  if (locked_)
146  throw Exception("This control space is locked. No further components can be added");
147 
148  components_.push_back(component);
149  componentCount_ = components_.size();
150 }
151 
153 {
154  return componentCount_;
155 }
156 
158 {
159  if (componentCount_ > index)
160  return components_[index];
161  else
162  throw Exception("Subspace index does not exist");
163 }
164 
166 {
167  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
168  if (components_[i]->getName() == name)
169  return components_[i];
170  throw Exception("Subspace " + name + " does not exist");
171 }
172 
174 {
175  unsigned int dim = 0;
176  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
177  dim += components_[i]->getDimension();
178  return dim;
179 }
180 
182 {
183  CompoundControl *control = new CompoundControl();
184  control->components = new Control*[componentCount_];
185  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
186  control->components[i] = components_[i]->allocControl();
187  return static_cast<Control*>(control);
188 }
189 
191 {
192  CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
193  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
194  components_[i]->freeControl(ccontrol->components[i]);
195  delete[] ccontrol->components;
196  delete ccontrol;
197 }
198 
199 void ompl::control::CompoundControlSpace::copyControl(Control *destination, const Control *source) const
200 {
201  CompoundControl *cdest = static_cast<CompoundControl*>(destination);
202  const CompoundControl *csrc = static_cast<const CompoundControl*>(source);
203  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
204  components_[i]->copyControl(cdest->components[i], csrc->components[i]);
205 }
206 
207 bool ompl::control::CompoundControlSpace::equalControls(const Control *control1, const Control *control2) const
208 {
209  const CompoundControl *ccontrol1 = static_cast<const CompoundControl*>(control1);
210  const CompoundControl *ccontrol2 = static_cast<const CompoundControl*>(control2);
211  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
212  if (!components_[i]->equalControls(ccontrol1->components[i], ccontrol2->components[i]))
213  return false;
214  return true;
215 }
216 
218 {
219  CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
220  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
221  components_[i]->nullControl(ccontrol->components[i]);
222 }
223 
225 {
227  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
228  ss->addSampler(components_[i]->allocControlSampler());
229  return ControlSamplerPtr(ss);
230 }
231 
233 {
234  locked_ = true;
235 }
236 
237 double* ompl::control::CompoundControlSpace::getValueAddressAtIndex(Control *control, const unsigned int index) const
238 {
239  CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
240  unsigned int idx = 0;
241 
242  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
243  for (unsigned int j = 0 ; j <= index ; ++j)
244  {
245  double *va = components_[i]->getValueAddressAtIndex(ccontrol->components[i], j);
246  if (va)
247  {
248  if (idx == index)
249  return va;
250  else
251  idx++;
252  }
253  else
254  break;
255  }
256  return NULL;
257 }
258 
259 void ompl::control::CompoundControlSpace::printControl(const Control *control, std::ostream &out) const
260 {
261  out << "Compound control [" << std::endl;
262  const CompoundControl *ccontrol = static_cast<const CompoundControl*>(control);
263  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
264  components_[i]->printControl(ccontrol->components[i], out);
265  out << "]" << std::endl;
266 }
267 
269 {
270  out << "Compound control space '" << getName() << "' [" << std::endl;
271  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
272  components_[i]->printSettings(out);
273  out << "]" << std::endl;
274 }
275 
277 {
278  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
279  components_[i]->setup();
281 }
282 
284 {
285  unsigned int l = 0;
286  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
287  l += components_[i]->getSerializationLength();
288  return l;
289 }
290 
291 void ompl::control::CompoundControlSpace::serialize(void *serialization, const Control *ctrl) const
292 {
293  const CompoundControl *compctrl = static_cast<const CompoundControl*>(ctrl);
294  unsigned int l = 0;
295  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
296  {
297  components_[i]->serialize(reinterpret_cast<char*>(serialization) + l, compctrl->components[i]);
298  l += components_[i]->getSerializationLength();
299  }
300 }
301 
302 void ompl::control::CompoundControlSpace::deserialize(Control *ctrl, const void *serialization) const
303 {
304  CompoundControl *compctrl = static_cast<CompoundControl*>(ctrl);
305  unsigned int l = 0;
306  for (unsigned int i = 0 ; i < componentCount_ ; ++i)
307  {
308  components_[i]->deserialize(compctrl->components[i], reinterpret_cast<const char*>(serialization) + l);
309  l += components_[i]->getSerializationLength();
310  }
311 }
312 
314 {
315  return true;
316 }
Unset type; this is the default type.
unsigned int getSubspaceCount() const
Get the number of control spaces that make up the compound control space.
virtual void serialize(void *serialization, const Control *ctrl) const
Serializes the given control into the serialization buffer.
base::StateSpacePtr stateSpace_
The state space controls can be applied to.
Definition: ControlSpace.h:184
const std::string & getName() const
Get the name of the control space.
Definition of a compound control sampler. This is useful to construct samplers for compound controls...
virtual void addSubspace(const ControlSpacePtr &component)
Adds a control space as a component of the compound control space.
Definition of an abstract control.
Definition: Control.h:48
virtual void addSampler(const ControlSamplerPtr &sampler)
Add a sampler as part of the new compound sampler. This sampler is used to sample part of the compoun...
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost shared pointer wrapper for ompl::control::ControlSampler.
virtual void deserialize(Control *ctrl, const void *serialization) const
Deserializes a control from the serialization buffer.
virtual void serialize(void *serialization, const Control *ctrl) const
Serializes the given control into the serialization buffer.
void lock()
Lock this control space. This means no further control spaces can be added as components. This function can be for instance called from the constructor of a state space that inherits from CompoundControlSpace to prevent the user to add further components.
const ControlSpacePtr & getSubspace(const unsigned int index) const
Get a specific subspace from the compound control space.
void setName(const std::string &name)
Set the name of the control space.
void clearControlSamplerAllocator()
Clear the control sampler allocator (reset to default)
virtual ControlSamplerPtr allocDefaultControlSampler() const =0
Allocate the default control sampler.
virtual unsigned int getDimension() const
Get the dimension of this control space.
virtual void copyControl(Control *destination, const Control *source) const =0
Copy a control to another.
virtual void printControl(const Control *control, std::ostream &out=std::cout) const
Print a control to a stream.
virtual void printSettings(std::ostream &out) const
Print the settings for this control space to a stream.
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...
A boost shared pointer wrapper for ompl::control::ControlSpace.
virtual void freeControl(Control *control) const =0
Free the memory of a control.
Control ** components
The components that make up a compound control.
Definition: Control.h:126
virtual bool isCompound() const
Check if the control space is compound.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation.
Definition of a compound control.
Definition: Control.h:93
virtual bool equalControls(const Control *control1, const Control *control2) const =0
Check if two controls are the same.
virtual void copyControl(Control *destination, const Control *source) const
Copy a control to another.
virtual unsigned int getSerializationLength() const
Returns the serialization size for a single control in this space.
int type_
A type assigned for this control space.
Definition: ControlSpace.h:181
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 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 ControlSamplerPtr allocDefaultControlSampler() const
Allocate the default control sampler.
ControlSpace(const base::StateSpacePtr &stateSpace)
Construct a control space, given the state space.
virtual bool equalControls(const Control *control1, const Control *control2) const
Check if two controls are the same.
virtual unsigned int getSerializationLength() const
Returns the serialization size for a single control in this space.
The exception type for ompl.
Definition: Exception.h:47
virtual void printControl(const Control *control, std::ostream &out) const
Print a control to a stream.
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.
void setControlSamplerAllocator(const ControlSamplerAllocator &csa)
Set the sampler allocator to use.
ControlSamplerAllocator csa_
An optional control sampler allocator.
Definition: ControlSpace.h:187
virtual bool isCompound() const
Check if the control space is compound.
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation.
virtual ControlSamplerPtr allocControlSampler() const
Allocate an instance of the control sampler for this space. This sampler will be allocated with the s...
virtual unsigned int getDimension() const =0
Get the dimension of this control space.
virtual void deserialize(Control *ctrl, const void *serialization) const
Deserializes a control from the serialization buffer.
virtual void nullControl(Control *control) const
Make the control have no effect if it were to be applied to a state for any amount of time...
virtual Control * allocControl() const
Allocate memory for a control.
virtual void printSettings(std::ostream &out) const
Print the settings for this control space to a stream.
virtual void freeControl(Control *control) const
Free the memory of a control.
boost::function< ControlSamplerPtr(const ControlSpace *)> ControlSamplerAllocator
Definition of a function that can allocate a control sampler.