37 #include "ompl/control/ControlSpace.h"
38 #include "ompl/util/Exception.h"
45 static void computeControlSpaceSignatureHelper(
const ControlSpace *space, std::vector<int> &signature)
47 signature.push_back(space->getType());
48 signature.push_back(space->getDimension());
50 if (space->isCompound())
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);
67 ompl::control::ControlSpace::~ControlSpace(
void)
90 return allocDefaultControlSampler();
110 out <<
"Control instance: " << control << std::endl;
115 out <<
"ControlSpace '" << getName() <<
"' instance: " <<
this << std::endl;
134 computeControlSpaceSignatureHelper(
this, signature);
135 signature.insert(signature.begin(), signature.size());
146 throw Exception(
"This control space is locked. No further components can be added");
148 components_.push_back(component);
149 componentCount_ = components_.size();
154 return componentCount_;
159 if (componentCount_ > index)
160 return components_[index];
162 throw Exception(
"Subspace index does not exist");
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");
175 unsigned int dim = 0;
176 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
177 dim += components_[i]->getDimension();
185 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
186 control->
components[i] = components_[i]->allocControl();
187 return static_cast<Control*
>(control);
193 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
194 components_[i]->freeControl(ccontrol->
components[i]);
203 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
211 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
220 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
221 components_[i]->nullControl(ccontrol->
components[i]);
227 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
228 ss->
addSampler(components_[i]->allocControlSampler());
240 unsigned int idx = 0;
242 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
243 for (
unsigned int j = 0 ; j <= index ; ++j)
245 double *va = components_[i]->getValueAddressAtIndex(ccontrol->
components[i], j);
261 out <<
"Compound control [" << std::endl;
263 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
264 components_[i]->printControl(ccontrol->
components[i], out);
265 out <<
"]" << std::endl;
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;
278 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
279 components_[i]->setup();
286 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
287 l += components_[i]->getSerializationLength();
295 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
297 components_[i]->serialize(reinterpret_cast<char*>(serialization) + l, compctrl->
components[i]);
298 l += components_[i]->getSerializationLength();
306 for (
unsigned int i = 0 ; i < componentCount_ ; ++i)
308 components_[i]->deserialize(compctrl->
components[i], reinterpret_cast<const char*>(serialization) + l);
309 l += components_[i]->getSerializationLength();