00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/control/ControlSpace.h"
00038 #include "ompl/util/Exception.h"
00039
00041 namespace ompl
00042 {
00043 namespace control
00044 {
00045 static void computeControlSpaceSignatureHelper(const ControlSpace *space, std::vector<int> &signature)
00046 {
00047 signature.push_back(space->getType());
00048 signature.push_back(space->getDimension());
00049
00050 if (space->isCompound())
00051 {
00052 unsigned int c = space->as<CompoundControlSpace>()->getSubspaceCount();
00053 for (unsigned int i = 0 ; i < c ; ++i)
00054 computeControlSpaceSignatureHelper(space->as<CompoundControlSpace>()->getSubspace(i).get(), signature);
00055 }
00056 }
00057 }
00058 }
00060
00061 ompl::control::ControlSpace::ControlSpace(const base::StateSpacePtr &stateSpace) : stateSpace_(stateSpace)
00062 {
00063 name_ = "Control[" + stateSpace_->getName() + "]";
00064 type_ = CONTROL_SPACE_UNKNOWN;
00065 }
00066
00067 ompl::control::ControlSpace::~ControlSpace(void)
00068 {
00069 }
00070
00071 const std::string& ompl::control::ControlSpace::getName(void) const
00072 {
00073 return name_;
00074 }
00075
00076 void ompl::control::ControlSpace::setName(const std::string &name)
00077 {
00078 name_ = name;
00079 }
00080
00081 void ompl::control::ControlSpace::setup(void)
00082 {
00083 }
00084
00085 ompl::control::ControlSamplerPtr ompl::control::ControlSpace::allocControlSampler(void) const
00086 {
00087 if (csa_)
00088 return csa_(this);
00089 else
00090 return allocDefaultControlSampler();
00091 }
00092
00093 void ompl::control::ControlSpace::setControlSamplerAllocator(const ControlSamplerAllocator &csa)
00094 {
00095 csa_ = csa;
00096 }
00097
00098 void ompl::control::ControlSpace::clearControlSamplerAllocator(void)
00099 {
00100 csa_ = ControlSamplerAllocator();
00101 }
00102
00103 double* ompl::control::ControlSpace::getValueAddressAtIndex(Control *control, const unsigned int index) const
00104 {
00105 return NULL;
00106 }
00107
00108 void ompl::control::ControlSpace::printControl(const Control *control, std::ostream &out) const
00109 {
00110 out << "Control instance: " << control << std::endl;
00111 }
00112
00113 void ompl::control::ControlSpace::printSettings(std::ostream &out) const
00114 {
00115 out << "ControlSpace '" << getName() << "' instance: " << this << std::endl;
00116 }
00117
00118 unsigned int ompl::control::ControlSpace::getSerializationLength(void) const
00119 {
00120 return 0;
00121 }
00122
00123 void ompl::control::ControlSpace::serialize(void *serialization, const Control *ctrl) const
00124 {
00125 }
00126
00127 void ompl::control::ControlSpace::deserialize(Control *ctrl, const void *serialization) const
00128 {
00129 }
00130
00131 void ompl::control::ControlSpace::computeSignature(std::vector<int> &signature) const
00132 {
00133 signature.clear();
00134 computeControlSpaceSignatureHelper(this, signature);
00135 signature.insert(signature.begin(), signature.size());
00136 }
00137
00138 bool ompl::control::ControlSpace::isCompound(void) const
00139 {
00140 return false;
00141 }
00142
00143 void ompl::control::CompoundControlSpace::addSubspace(const ControlSpacePtr &component)
00144 {
00145 if (locked_)
00146 throw Exception("This control space is locked. No further components can be added");
00147
00148 components_.push_back(component);
00149 componentCount_ = components_.size();
00150 }
00151
00152 unsigned int ompl::control::CompoundControlSpace::getSubspaceCount(void) const
00153 {
00154 return componentCount_;
00155 }
00156
00157 const ompl::control::ControlSpacePtr& ompl::control::CompoundControlSpace::getSubspace(const unsigned int index) const
00158 {
00159 if (componentCount_ > index)
00160 return components_[index];
00161 else
00162 throw Exception("Subspace index does not exist");
00163 }
00164
00165 const ompl::control::ControlSpacePtr& ompl::control::CompoundControlSpace::getSubspace(const std::string &name) const
00166 {
00167 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00168 if (components_[i]->getName() == name)
00169 return components_[i];
00170 throw Exception("Subspace " + name + " does not exist");
00171 }
00172
00173 unsigned int ompl::control::CompoundControlSpace::getDimension(void) const
00174 {
00175 unsigned int dim = 0;
00176 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00177 dim += components_[i]->getDimension();
00178 return dim;
00179 }
00180
00181 ompl::control::Control* ompl::control::CompoundControlSpace::allocControl(void) const
00182 {
00183 CompoundControl *control = new CompoundControl();
00184 control->components = new Control*[componentCount_];
00185 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00186 control->components[i] = components_[i]->allocControl();
00187 return static_cast<Control*>(control);
00188 }
00189
00190 void ompl::control::CompoundControlSpace::freeControl(Control *control) const
00191 {
00192 CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
00193 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00194 components_[i]->freeControl(ccontrol->components[i]);
00195 delete[] ccontrol->components;
00196 delete ccontrol;
00197 }
00198
00199 void ompl::control::CompoundControlSpace::copyControl(Control *destination, const Control *source) const
00200 {
00201 CompoundControl *cdest = static_cast<CompoundControl*>(destination);
00202 const CompoundControl *csrc = static_cast<const CompoundControl*>(source);
00203 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00204 components_[i]->copyControl(cdest->components[i], csrc->components[i]);
00205 }
00206
00207 bool ompl::control::CompoundControlSpace::equalControls(const Control *control1, const Control *control2) const
00208 {
00209 const CompoundControl *ccontrol1 = static_cast<const CompoundControl*>(control1);
00210 const CompoundControl *ccontrol2 = static_cast<const CompoundControl*>(control2);
00211 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00212 if (!components_[i]->equalControls(ccontrol1->components[i], ccontrol2->components[i]))
00213 return false;
00214 return true;
00215 }
00216
00217 void ompl::control::CompoundControlSpace::nullControl(Control *control) const
00218 {
00219 CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
00220 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00221 components_[i]->nullControl(ccontrol->components[i]);
00222 }
00223
00224 ompl::control::ControlSamplerPtr ompl::control::CompoundControlSpace::allocDefaultControlSampler(void) const
00225 {
00226 CompoundControlSampler *ss = new CompoundControlSampler(this);
00227 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00228 ss->addSampler(components_[i]->allocControlSampler());
00229 return ControlSamplerPtr(ss);
00230 }
00231
00232 void ompl::control::CompoundControlSpace::lock(void)
00233 {
00234 locked_ = true;
00235 }
00236
00237 double* ompl::control::CompoundControlSpace::getValueAddressAtIndex(Control *control, const unsigned int index) const
00238 {
00239 CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
00240 unsigned int idx = 0;
00241
00242 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00243 for (unsigned int j = 0 ; j <= index ; ++j)
00244 {
00245 double *va = components_[i]->getValueAddressAtIndex(ccontrol->components[i], j);
00246 if (va)
00247 {
00248 if (idx == index)
00249 return va;
00250 else
00251 idx++;
00252 }
00253 else
00254 break;
00255 }
00256 return NULL;
00257 }
00258
00259 void ompl::control::CompoundControlSpace::printControl(const Control *control, std::ostream &out) const
00260 {
00261 out << "Compound control [" << std::endl;
00262 const CompoundControl *ccontrol = static_cast<const CompoundControl*>(control);
00263 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00264 components_[i]->printControl(ccontrol->components[i], out);
00265 out << "]" << std::endl;
00266 }
00267
00268 void ompl::control::CompoundControlSpace::printSettings(std::ostream &out) const
00269 {
00270 out << "Compound control space '" << getName() << "' [" << std::endl;
00271 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00272 components_[i]->printSettings(out);
00273 out << "]" << std::endl;
00274 }
00275
00276 void ompl::control::CompoundControlSpace::setup(void)
00277 {
00278 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00279 components_[i]->setup();
00280 ControlSpace::setup();
00281 }
00282
00283 unsigned int ompl::control::CompoundControlSpace::getSerializationLength(void) const
00284 {
00285 unsigned int l = 0;
00286 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00287 l += components_[i]->getSerializationLength();
00288 return l;
00289 }
00290
00291 void ompl::control::CompoundControlSpace::serialize(void *serialization, const Control *ctrl) const
00292 {
00293 const CompoundControl *compctrl = static_cast<const CompoundControl*>(ctrl);
00294 unsigned int l = 0;
00295 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00296 {
00297 components_[i]->serialize(reinterpret_cast<char*>(serialization) + l, compctrl->components[i]);
00298 l += components_[i]->getSerializationLength();
00299 }
00300 }
00301
00302 void ompl::control::CompoundControlSpace::deserialize(Control *ctrl, const void *serialization) const
00303 {
00304 CompoundControl *compctrl = static_cast<CompoundControl*>(ctrl);
00305 unsigned int l = 0;
00306 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00307 {
00308 components_[i]->deserialize(compctrl->components[i], reinterpret_cast<const char*>(serialization) + l);
00309 l += components_[i]->getSerializationLength();
00310 }
00311 }
00312
00313 bool ompl::control::CompoundControlSpace::isCompound(void) const
00314 {
00315 return true;
00316 }