ompl/control/src/ControlSpace.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 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() 00068 { 00069 } 00070 00071 const std::string& ompl::control::ControlSpace::getName() 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() 00082 { 00083 } 00084 00085 ompl::control::ControlSamplerPtr ompl::control::ControlSpace::allocControlSampler() 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() 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() 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() 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() 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() 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() 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() 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() 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() 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() 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() const 00314 { 00315 return true; 00316 }