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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines