All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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 
00040 ompl::control::ControlSpace::ControlSpace(const base::StateSpacePtr &stateSpace) : stateSpace_(stateSpace)
00041 {
00042     name_ = "Control[" + stateSpace_->getName() + "]";
00043     type_ = CONTROL_SPACE_UNKNOWN;
00044 }
00045 
00046 ompl::control::ControlSpace::~ControlSpace(void)
00047 {
00048 }
00049 
00050 const std::string& ompl::control::ControlSpace::getName(void) const
00051 {
00052     return name_;
00053 }
00054 
00055 void ompl::control::ControlSpace::setName(const std::string &name)
00056 {
00057     name_ = name;
00058 }
00059 
00060 void ompl::control::ControlSpace::setup(void)
00061 {
00062 }
00063 
00064 ompl::control::ControlSamplerPtr ompl::control::ControlSpace::allocControlSampler(void) const
00065 {
00066     if (csa_)
00067         return csa_(this);
00068     else
00069         return allocDefaultControlSampler();
00070 }
00071 
00072 void ompl::control::ControlSpace::setControlSamplerAllocator(const ControlSamplerAllocator &csa)
00073 {
00074     csa_ = csa;
00075 }
00076 
00077 void ompl::control::ControlSpace::clearControlSamplerAllocator(void)
00078 {
00079     csa_ = ControlSamplerAllocator();
00080 }
00081 
00082 double* ompl::control::ControlSpace::getValueAddressAtIndex(Control *control, const unsigned int index) const
00083 {
00084     return NULL;
00085 }
00086 
00087 void ompl::control::ControlSpace::printControl(const Control *control, std::ostream &out) const
00088 {
00089     out << "Control instance: " << control << std::endl;
00090 }
00091 
00092 void ompl::control::ControlSpace::printSettings(std::ostream &out) const
00093 {
00094     out << "ControlSpace '" << getName() << "' instance: " << this << std::endl;
00095 }
00096 
00097 void ompl::control::CompoundControlSpace::addSubSpace(const ControlSpacePtr &component)
00098 {
00099     if (locked_)
00100         throw Exception("This control space is locked. No further components can be added");
00101 
00102     components_.push_back(component);
00103     componentCount_ = components_.size();
00104 }
00105 
00106 unsigned int ompl::control::CompoundControlSpace::getSubSpaceCount(void) const
00107 {
00108     return componentCount_;
00109 }
00110 
00111 const ompl::control::ControlSpacePtr& ompl::control::CompoundControlSpace::getSubSpace(const unsigned int index) const
00112 {
00113     if (componentCount_ > index)
00114         return components_[index];
00115     else
00116         throw Exception("Subspace index does not exist");
00117 }
00118 
00119 const ompl::control::ControlSpacePtr& ompl::control::CompoundControlSpace::getSubSpace(const std::string &name) const
00120 {
00121     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00122         if (components_[i]->getName() == name)
00123             return components_[i];
00124     throw Exception("Subspace " + name + " does not exist");
00125 }
00126 
00127 unsigned int ompl::control::CompoundControlSpace::getDimension(void) const
00128 {
00129     unsigned int dim = 0;
00130     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00131         dim += components_[i]->getDimension();
00132     return dim;
00133 }
00134 
00135 ompl::control::Control* ompl::control::CompoundControlSpace::allocControl(void) const
00136 {
00137     CompoundControl *control = new CompoundControl();
00138     control->components = new Control*[componentCount_];
00139     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00140         control->components[i] = components_[i]->allocControl();
00141     return static_cast<Control*>(control);
00142 }
00143 
00144 void ompl::control::CompoundControlSpace::freeControl(Control *control) const
00145 {
00146     CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
00147     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00148         components_[i]->freeControl(ccontrol->components[i]);
00149     delete[] ccontrol->components;
00150     delete ccontrol;
00151 }
00152 
00153 void ompl::control::CompoundControlSpace::copyControl(Control *destination, const Control *source) const
00154 {
00155     CompoundControl      *cdest = static_cast<CompoundControl*>(destination);
00156     const CompoundControl *csrc = static_cast<const CompoundControl*>(source);
00157     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00158         components_[i]->copyControl(cdest->components[i], csrc->components[i]);
00159 }
00160 
00161 bool ompl::control::CompoundControlSpace::equalControls(const Control *control1, const Control *control2) const
00162 {
00163     const CompoundControl *ccontrol1 = static_cast<const CompoundControl*>(control1);
00164     const CompoundControl *ccontrol2 = static_cast<const CompoundControl*>(control2);
00165     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00166         if (!components_[i]->equalControls(ccontrol1->components[i], ccontrol2->components[i]))
00167             return false;
00168     return true;
00169 }
00170 
00171 void ompl::control::CompoundControlSpace::nullControl(Control *control) const
00172 {
00173     CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
00174     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00175         components_[i]->nullControl(ccontrol->components[i]);
00176 }
00177 
00178 ompl::control::ControlSamplerPtr ompl::control::CompoundControlSpace::allocDefaultControlSampler(void) const
00179 {
00180     CompoundControlSampler *ss = new CompoundControlSampler(this);
00181     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00182         ss->addSampler(components_[i]->allocControlSampler());
00183     return ControlSamplerPtr(ss);
00184 }
00185 
00186 void ompl::control::CompoundControlSpace::lock(void)
00187 {
00188     locked_ = true;
00189 }
00190 
00191 double* ompl::control::CompoundControlSpace::getValueAddressAtIndex(Control *control, const unsigned int index) const
00192 {
00193     CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
00194     unsigned int idx = 0;
00195 
00196     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00197         for (unsigned int j = 0 ; j <= index ; ++j)
00198         {
00199             double *va = components_[i]->getValueAddressAtIndex(ccontrol->components[i], j);
00200             if (va)
00201             {
00202                 if (idx == index)
00203                     return va;
00204                 else
00205                     idx++;
00206             }
00207             else
00208                 break;
00209         }
00210     return NULL;
00211 }
00212 
00213 void ompl::control::CompoundControlSpace::printControl(const Control *control, std::ostream &out) const
00214 {
00215     out << "Compound control [" << std::endl;
00216     const CompoundControl *ccontrol = static_cast<const CompoundControl*>(control);
00217     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00218         components_[i]->printControl(ccontrol->components[i], out);
00219     out << "]" << std::endl;
00220 }
00221 
00222 void ompl::control::CompoundControlSpace::printSettings(std::ostream &out) const
00223 {
00224     out << "Compound control space '" << getName() << "' [" << std::endl;
00225     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00226         components_[i]->printSettings(out);
00227     out << "]" << std::endl;
00228 }
00229 
00230 void ompl::control::CompoundControlSpace::setup(void)
00231 {
00232     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00233         components_[i]->setup();
00234     ControlSpace::setup();
00235 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends