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