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();
Unset type; this is the default type.
virtual void serialize(void *serialization, const Control *ctrl) const
Serializes the given control into the serialization buffer.
void clearControlSamplerAllocator(void)
Clear the control sampler allocator (reset to default)
base::StateSpacePtr stateSpace_
The state space controls can be applied to.
virtual unsigned int getDimension(void) const
Get the dimension of this control space.
Definition of a compound control sampler. This is useful to construct samplers for compound controls...
virtual void addSubspace(const ControlSpacePtr &component)
Adds a control space as a component of the compound control space.
Definition of an abstract control.
virtual void addSampler(const ControlSamplerPtr &sampler)
Add a sampler as part of the new compound sampler. This sampler is used to sample part of the compoun...
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost shared pointer wrapper for ompl::control::ControlSampler.
virtual unsigned int getSerializationLength(void) const
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.
virtual void serialize(void *serialization, const Control *ctrl) const
Serializes the given control into the serialization buffer.
virtual bool isCompound(void) const
Check if the control space is compound.
const ControlSpacePtr & getSubspace(const unsigned int index) const
Get a specific subspace from the compound control space.
virtual void setup(void)
Perform final setup steps. This function is automatically called by the SpaceInformation.
void setName(const std::string &name)
Set the name of the control space.
virtual ControlSamplerPtr allocDefaultControlSampler(void) const
Allocate the default control sampler.
unsigned int getSubspaceCount(void) const
Get the number of control spaces that make up the compound control space.
virtual void printControl(const Control *control, std::ostream &out=std::cout) const
Print a control to a stream.
virtual void printSettings(std::ostream &out) const
Print the settings for this control space to a stream.
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...
A boost shared pointer wrapper for ompl::control::ControlSpace.
Control ** components
The components that make up a compound control.
Definition of a compound control.
const std::string & getName(void) const
Get the name of the control space.
virtual void copyControl(Control *destination, const Control *source) const
Copy a control to another.
int type_
A type assigned for 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...
virtual void setup(void)
Perform final setup steps. This function is automatically called by the SpaceInformation.
ControlSpace(const base::StateSpacePtr &stateSpace)
Construct a control space, given the state space.
virtual bool equalControls(const Control *control1, const Control *control2) const
Check if two controls are the same.
virtual unsigned int getSerializationLength(void) const
Returns the serialization size for a single control in this space.
The exception type for ompl.
virtual void printControl(const Control *control, std::ostream &out) const
Print a control to a stream.
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.
void setControlSamplerAllocator(const ControlSamplerAllocator &csa)
Set the sampler allocator to use.
virtual bool isCompound(void) const
Check if the control space is compound.
virtual Control * allocControl(void) const
Allocate memory for a control.
void lock(void)
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 ControlSamplerPtr allocControlSampler(void) const
Allocate an instance of the control sampler for this space. This sampler will be allocated with the s...
virtual void deserialize(Control *ctrl, const void *serialization) const
Deserializes a control from the serialization buffer.
virtual void nullControl(Control *control) const
Make the control have no effect if it were to be applied to a state for any amount of time...
virtual void printSettings(std::ostream &out) const
Print the settings for this control space to a stream.
virtual void freeControl(Control *control) const
Free the memory of a control.
boost::function< ControlSamplerPtr(const ControlSpace *)> ControlSamplerAllocator
Definition of a function that can allocate a control sampler.