ompl/control/spaces/src/RealVectorControlSpace.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/spaces/RealVectorControlSpace.h"
00038 #include "ompl/util/Exception.h"
00039 #include <boost/lexical_cast.hpp>
00040 #include <cstring>
00041 #include <limits>
00042 
00043 void ompl::control::RealVectorControlUniformSampler::sample(Control *control)
00044 {
00045     const unsigned int dim = space_->getDimension();
00046     const base::RealVectorBounds &bounds = static_cast<const RealVectorControlSpace*>(space_)->getBounds();
00047 
00048     RealVectorControlSpace::ControlType *rcontrol = static_cast<RealVectorControlSpace::ControlType*>(control);
00049     for (unsigned int i = 0 ; i < dim ; ++i)
00050         rcontrol->values[i] = rng_.uniformReal(bounds.low[i], bounds.high[i]);
00051 }
00052 
00053 void ompl::control::RealVectorControlSpace::setup()
00054 {
00055     ControlSpace::setup();
00056     bounds_.check();
00057 }
00058 
00059 void ompl::control::RealVectorControlSpace::setBounds(const base::RealVectorBounds &bounds)
00060 {
00061     bounds.check();
00062     if (bounds.low.size() != dimension_)
00063         throw Exception("Bounds do not match dimension of control space: expected dimension " +
00064                         boost::lexical_cast<std::string>(dimension_) + " but got dimension " +
00065                         boost::lexical_cast<std::string>(bounds.low.size()));
00066     bounds_ = bounds;
00067 }
00068 
00069 unsigned int ompl::control::RealVectorControlSpace::getDimension() const
00070 {
00071     return dimension_;
00072 }
00073 
00074 void ompl::control::RealVectorControlSpace::copyControl(Control *destination, const Control *source) const
00075 {
00076     memcpy(static_cast<ControlType*>(destination)->values,
00077            static_cast<const ControlType*>(source)->values, controlBytes_);
00078 }
00079 
00080 bool ompl::control::RealVectorControlSpace::equalControls(const Control *control1, const Control *control2) const
00081 {
00082     const double *s1 = static_cast<const ControlType*>(control1)->values;
00083     const double *s2 = static_cast<const ControlType*>(control2)->values;
00084     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00085     {
00086         double diff = (*s1++) - (*s2++);
00087         if (fabs(diff) > std::numeric_limits<double>::epsilon() * 2.0)
00088             return false;
00089     }
00090     return true;
00091 }
00092 
00093 ompl::control::ControlSamplerPtr ompl::control::RealVectorControlSpace::allocDefaultControlSampler() const
00094 {
00095     return ControlSamplerPtr(new RealVectorControlUniformSampler(this));
00096 }
00097 
00098 ompl::control::Control* ompl::control::RealVectorControlSpace::allocControl() const
00099 {
00100     ControlType *rcontrol = new ControlType();
00101     rcontrol->values = new double[dimension_];
00102     return rcontrol;
00103 }
00104 
00105 void ompl::control::RealVectorControlSpace::freeControl(Control *control) const
00106 {
00107     ControlType *rcontrol = static_cast<ControlType*>(control);
00108     delete[] rcontrol->values;
00109     delete rcontrol;
00110 }
00111 
00112 void ompl::control::RealVectorControlSpace::nullControl(Control *control) const
00113 {
00114     ControlType *rcontrol = static_cast<ControlType*>(control);
00115     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00116     {
00117         if (bounds_.low[i] <= 0.0 && bounds_.high[i] >= 0.0)
00118             rcontrol->values[i] = 0.0;
00119         else
00120             rcontrol->values[i] = bounds_.low[i];
00121     }
00122 }
00123 
00124 double* ompl::control::RealVectorControlSpace::getValueAddressAtIndex(Control *control, const unsigned int index) const
00125 {
00126     return index < dimension_ ? static_cast<ControlType*>(control)->values + index : NULL;
00127 }
00128 
00129 void ompl::control::RealVectorControlSpace::printControl(const Control *control, std::ostream &out) const
00130 {
00131     out << "RealVectorControl [";
00132     if (control)
00133     {
00134         const ControlType *rcontrol = static_cast<const ControlType*>(control);
00135         for (unsigned int i = 0 ; i < dimension_ ; ++i)
00136         {
00137             out << rcontrol->values[i];
00138             if (i + 1 < dimension_)
00139                 out << ' ';
00140         }
00141     }
00142     else
00143         out << "NULL";
00144     out << ']' << std::endl;
00145 }
00146 
00147 void ompl::control::RealVectorControlSpace::printSettings(std::ostream &out) const
00148 {
00149     out << "Real vector control space '" << getName() << "' with bounds: " << std::endl;
00150     out << "  - min: ";
00151     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00152         out << bounds_.low[i] << " ";
00153     out << std::endl;
00154     out << "  - max: ";
00155     for (unsigned int i = 0 ; i < dimension_ ; ++i)
00156         out << bounds_.high[i] << " ";
00157     out << std::endl;
00158 }
00159 
00160 unsigned int ompl::control::RealVectorControlSpace::getSerializationLength() const
00161 {
00162     return controlBytes_;
00163 }
00164 
00165 void ompl::control::RealVectorControlSpace::serialize(void *serialization, const Control *ctrl) const
00166 {
00167     memcpy(serialization, ctrl->as<ControlType>()->values, controlBytes_);
00168 }
00169 
00170 void ompl::control::RealVectorControlSpace::deserialize(Control *ctrl, const void *serialization) const
00171 {
00172     memcpy(ctrl->as<ControlType>()->values, serialization, controlBytes_);
00173 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines