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);
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 nullptr;
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  throw Exception("Subspace index does not exist");
162 }
163 
165 {
166  for (unsigned int i = 0; i < componentCount_; ++i)
167  if (components_[i]->getName() == name)
168  return components_[i];
169  throw Exception("Subspace " + name + " does not exist");
170 }
171 
173 {
174  unsigned int dim = 0;
175  for (unsigned int i = 0; i < componentCount_; ++i)
176  dim += components_[i]->getDimension();
177  return dim;
178 }
179 
181 {
182  auto *control = new CompoundControl();
183  control->components = new Control *[componentCount_];
184  for (unsigned int i = 0; i < componentCount_; ++i)
185  control->components[i] = components_[i]->allocControl();
186  return static_cast<Control *>(control);
187 }
188 
190 {
191  auto *ccontrol = static_cast<CompoundControl *>(control);
192  for (unsigned int i = 0; i < componentCount_; ++i)
193  components_[i]->freeControl(ccontrol->components[i]);
194  delete[] ccontrol->components;
195  delete ccontrol;
196 }
197 
198 void ompl::control::CompoundControlSpace::copyControl(Control *destination, const Control *source) const
199 {
200  auto *cdest = static_cast<CompoundControl *>(destination);
201  const auto *csrc = static_cast<const CompoundControl *>(source);
202  for (unsigned int i = 0; i < componentCount_; ++i)
203  components_[i]->copyControl(cdest->components[i], csrc->components[i]);
204 }
205 
206 bool ompl::control::CompoundControlSpace::equalControls(const Control *control1, const Control *control2) const
207 {
208  const auto *ccontrol1 = static_cast<const CompoundControl *>(control1);
209  const auto *ccontrol2 = static_cast<const CompoundControl *>(control2);
210  for (unsigned int i = 0; i < componentCount_; ++i)
211  if (!components_[i]->equalControls(ccontrol1->components[i], ccontrol2->components[i]))
212  return false;
213  return true;
214 }
215 
217 {
218  auto *ccontrol = static_cast<CompoundControl *>(control);
219  for (unsigned int i = 0; i < componentCount_; ++i)
220  components_[i]->nullControl(ccontrol->components[i]);
221 }
222 
224 {
225  auto ss(std::make_shared<CompoundControlSampler>(this));
226  for (unsigned int i = 0; i < componentCount_; ++i)
227  ss->addSampler(components_[i]->allocControlSampler());
228  return ss;
229 }
230 
232 {
233  locked_ = true;
234 }
235 
236 double *ompl::control::CompoundControlSpace::getValueAddressAtIndex(Control *control, const unsigned int index) const
237 {
238  auto *ccontrol = static_cast<CompoundControl *>(control);
239  unsigned int idx = 0;
240 
241  for (unsigned int i = 0; i < componentCount_; ++i)
242  for (unsigned int j = 0; j <= index; ++j)
243  {
244  double *va = components_[i]->getValueAddressAtIndex(ccontrol->components[i], j);
245  if (va != nullptr)
246  {
247  if (idx == index)
248  return va;
249  idx++;
250  }
251  else
252  break;
253  }
254  return nullptr;
255 }
256 
257 void ompl::control::CompoundControlSpace::printControl(const Control *control, std::ostream &out) const
258 {
259  out << "Compound control [" << std::endl;
260  const auto *ccontrol = static_cast<const CompoundControl *>(control);
261  for (unsigned int i = 0; i < componentCount_; ++i)
262  components_[i]->printControl(ccontrol->components[i], out);
263  out << "]" << std::endl;
264 }
265 
267 {
268  out << "Compound control space '" << getName() << "' [" << std::endl;
269  for (unsigned int i = 0; i < componentCount_; ++i)
270  components_[i]->printSettings(out);
271  out << "]" << std::endl;
272 }
273 
275 {
276  for (unsigned int i = 0; i < componentCount_; ++i)
277  components_[i]->setup();
279 }
280 
282 {
283  unsigned int l = 0;
284  for (unsigned int i = 0; i < componentCount_; ++i)
285  l += components_[i]->getSerializationLength();
286  return l;
287 }
288 
289 void ompl::control::CompoundControlSpace::serialize(void *serialization, const Control *ctrl) const
290 {
291  const auto *compctrl = static_cast<const CompoundControl *>(ctrl);
292  unsigned int l = 0;
293  for (unsigned int i = 0; i < componentCount_; ++i)
294  {
295  components_[i]->serialize(reinterpret_cast<char *>(serialization) + l, compctrl->components[i]);
296  l += components_[i]->getSerializationLength();
297  }
298 }
299 
300 void ompl::control::CompoundControlSpace::deserialize(Control *ctrl, const void *serialization) const
301 {
302  auto *compctrl = static_cast<CompoundControl *>(ctrl);
303  unsigned int l = 0;
304  for (unsigned int i = 0; i < componentCount_; ++i)
305  {
306  components_[i]->deserialize(compctrl->components[i], reinterpret_cast<const char *>(serialization) + l);
307  l += components_[i]->getSerializationLength();
308  }
309 }
310 
312 {
313  return true;
314 }
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.
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...
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.
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
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...
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.
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...
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.
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.
const ControlSpacePtr & getSubspace(unsigned int index) const
Get a specific subspace from the compound control space.