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  return allocDefaultControlSampler();
91 }
92 
94 {
95  csa_ = csa;
96 }
97 
99 {
100  csa_ = ControlSamplerAllocator();
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 (const auto &component : components_)
167  if (component->getName() == name)
168  return component;
169  throw Exception("Subspace " + name + " does not exist");
170 }
171 
173 {
174  unsigned int dim = 0;
175  for (const auto &component : components_)
176  dim += component->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 (const auto &component : components_)
227  ss->addSampler(component->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 (const auto &component : components_)
277  component->setup();
279 }
280 
282 {
283  unsigned int l = 0;
284  for (const auto &component : components_)
285  l += component->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 }
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
std::function< ControlSamplerPtr(const ControlSpace *)> ControlSamplerAllocator
Definition of a function that can allocate a control sampler.
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....
@ CONTROL_SPACE_UNKNOWN
Unset type; this is the default type.
void freeControl(Control *control) const override
Free the memory of 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.
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
void printSettings(std::ostream &out) const override
Print the settings for this control space to a stream.
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 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.
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.
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.
void copyControl(Control *destination, const Control *source) const override
Copy a control to another.
The exception type for ompl.
Definition: Exception.h:78
Control ** components
The components that make up a compound control.
Definition: Control.h:182
bool equalControls(const Control *control1, const Control *control2) const override
Check if two controls are the same.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21