All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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(void)
68 {
69 }
70 
71 const std::string& ompl::control::ControlSpace::getName(void) 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
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 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 }