All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
ControlManifold.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/ControlManifold.h"
00038 #include "ompl/util/Exception.h"
00039 
00040 ompl::control::ControlManifold::ControlManifold(const base::StateManifoldPtr &stateManifold) : stateManifold_(stateManifold)
00041 {
00042     name_ = "Control[" + stateManifold_->getName() + "]";
00043 }
00044 
00045 ompl::control::ControlManifold::~ControlManifold(void)
00046 {
00047 }
00048 
00049 const std::string& ompl::control::ControlManifold::getName(void) const
00050 {
00051     return name_;
00052 }
00053 
00054 void ompl::control::ControlManifold::setName(const std::string &name)
00055 {
00056     name_ = name;
00057 }
00058 
00059 void ompl::control::ControlManifold::setup(void)
00060 {
00061 }
00062 
00063 bool ompl::control::ControlManifold::canPropagateBackward(void) const
00064 {
00065     return true;
00066 }
00067 
00068 double* ompl::control::ControlManifold::getValueAddressAtIndex(Control *control, const unsigned int index) const
00069 {
00070     return NULL;
00071 }
00072 
00073 void ompl::control::ControlManifold::printControl(const Control *control, std::ostream &out) const
00074 {
00075     out << "Control instance: " << control << std::endl;
00076 }
00077 
00078 void ompl::control::ControlManifold::printSettings(std::ostream &out) const
00079 {
00080     out << "ControlManifold '" << getName() << "' instance: " << this << std::endl;
00081 }
00082 
00083 void ompl::control::ControlManifold::propagate(const base::State *state, const Control* control, const double duration, base::State *result) const
00084 {
00085     if (statePropagation_)
00086         statePropagation_(state, control, duration, result);
00087     else
00088         throw Exception("State propagation routine is not set for control manifold. Either set this routine or provide a different implementation in an inherited class.");
00089 }
00090 
00091 void ompl::control::ControlManifold::setPropagationFunction(const StatePropagationFn &fn)
00092 {
00093     statePropagation_ = fn;
00094 }
00095 
00096 void ompl::control::CompoundControlManifold::addSubManifold(const ControlManifoldPtr &component)
00097 {
00098     if (locked_)
00099         throw Exception("This manifold is locked. No further components can be added");
00100 
00101     components_.push_back(component);
00102     componentCount_ = components_.size();
00103 }
00104 
00105 unsigned int ompl::control::CompoundControlManifold::getSubManifoldCount(void) const
00106 {
00107     return componentCount_;
00108 }
00109 
00110 const ompl::control::ControlManifoldPtr& ompl::control::CompoundControlManifold::getSubManifold(const unsigned int index) const
00111 {
00112     if (componentCount_ > index)
00113         return components_[index];
00114     else
00115         throw Exception("Submanifold index does not exist");
00116 }
00117 
00118 const ompl::control::ControlManifoldPtr& ompl::control::CompoundControlManifold::getSubManifold(const std::string &name) const
00119 {
00120     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00121         if (components_[i]->getName() == name)
00122             return components_[i];
00123     throw Exception("Submanifold " + name + " does not exist");
00124 }
00125 
00126 unsigned int ompl::control::CompoundControlManifold::getDimension(void) const
00127 {
00128     unsigned int dim = 0;
00129     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00130         dim += components_[i]->getDimension();
00131     return dim;
00132 }
00133 
00134 ompl::control::Control* ompl::control::CompoundControlManifold::allocControl(void) const
00135 {
00136     CompoundControl *control = new CompoundControl();
00137     control->components = new Control*[componentCount_];
00138     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00139         control->components[i] = components_[i]->allocControl();
00140     return static_cast<Control*>(control);
00141 }
00142 
00143 void ompl::control::CompoundControlManifold::freeControl(Control *control) const
00144 {
00145     CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
00146     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00147         components_[i]->freeControl(ccontrol->components[i]);
00148     delete[] ccontrol->components;
00149     delete ccontrol;
00150 }
00151 
00152 void ompl::control::CompoundControlManifold::copyControl(Control *destination, const Control *source) const
00153 {
00154     CompoundControl      *cdest = static_cast<CompoundControl*>(destination);
00155     const CompoundControl *csrc = static_cast<const CompoundControl*>(source);
00156     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00157         components_[i]->copyControl(cdest->components[i], csrc->components[i]);
00158 }
00159 
00160 bool ompl::control::CompoundControlManifold::equalControls(const Control *control1, const Control *control2) const
00161 {
00162     const CompoundControl *ccontrol1 = static_cast<const CompoundControl*>(control1);
00163     const CompoundControl *ccontrol2 = static_cast<const CompoundControl*>(control2);
00164     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00165         if (!components_[i]->equalControls(ccontrol1->components[i], ccontrol2->components[i]))
00166             return false;
00167     return true;
00168 }
00169 
00170 void ompl::control::CompoundControlManifold::nullControl(Control *control) const
00171 {
00172     CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
00173     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00174         components_[i]->nullControl(ccontrol->components[i]);
00175 }
00176 
00177 ompl::control::ControlSamplerPtr ompl::control::CompoundControlManifold::allocControlSampler(void) const
00178 {
00179     CompoundControlSampler *ss = new CompoundControlSampler(this);
00180     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00181         ss->addSampler(components_[i]->allocControlSampler());
00182     return ControlSamplerPtr(ss);
00183 }
00184 
00185 void ompl::control::CompoundControlManifold::propagate(const base::State *state, const Control* control, const double duration, base::State *result) const
00186 {
00187     if (statePropagation_)
00188         statePropagation_(state, control, duration, result);
00189     else
00190     {
00191         const base::CompoundState *cstate = static_cast<const base::CompoundState*>(state);
00192         const CompoundControl *ccontrol = static_cast<const CompoundControl*>(control);
00193         base::CompoundState *cresult = static_cast<base::CompoundState*>(result);
00194         for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00195             components_[i]->propagate(cstate->components[i], ccontrol->components[i], duration, cresult->components[i]);
00196     }
00197 }
00198 
00199 void ompl::control::CompoundControlManifold::lock(void)
00200 {
00201     locked_ = true;
00202 }
00203 
00204 bool ompl::control::CompoundControlManifold::canPropagateBackward(void) const
00205 {
00206     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00207         if (!components_[i]->canPropagateBackward())
00208             return false;
00209     return true;
00210 }
00211 
00212 double* ompl::control::CompoundControlManifold::getValueAddressAtIndex(Control *control, const unsigned int index) const
00213 {
00214     CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
00215     unsigned int idx = 0;
00216 
00217     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00218         for (unsigned int j = 0 ; j <= index ; ++j)
00219         {
00220             double *va = components_[i]->getValueAddressAtIndex(ccontrol->components[i], j);
00221             if (va)
00222             {
00223                 if (idx == index)
00224                     return va;
00225                 else
00226                     idx++;
00227             }
00228             else
00229                 break;
00230         }
00231     return NULL;
00232 }
00233 
00234 void ompl::control::CompoundControlManifold::printControl(const Control *control, std::ostream &out) const
00235 {
00236     out << "Compound control [" << std::endl;
00237     const CompoundControl *ccontrol = static_cast<const CompoundControl*>(control);
00238     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00239         components_[i]->printControl(ccontrol->components[i], out);
00240     out << "]" << std::endl;
00241 }
00242 
00243 void ompl::control::CompoundControlManifold::printSettings(std::ostream &out) const
00244 {
00245     out << "Compound control manifold '" << getName() << "' [" << std::endl;
00246     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00247         components_[i]->printSettings(out);
00248     out << "]" << std::endl;
00249 }
00250 
00251 void ompl::control::CompoundControlManifold::setup(void)
00252 {
00253     for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00254         components_[i]->setup();
00255     ControlManifold::setup();
00256 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator