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