demos/Koules/KoulesDirectedControlSampler.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, 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: Beck Chen, Mark Moll */
00036 
00037 #include "KoulesDirectedControlSampler.h"
00038 #include "KoulesStateSpace.h"
00039 #include <ompl/base/spaces/RealVectorStateSpace.h>
00040 
00041 namespace ob = ompl::base;
00042 namespace oc = ompl::control;
00043 
00044 unsigned int KoulesDirectedControlSampler::sampleTo(oc::Control *control, const ob::State *source, ob::State *dest)
00045 {
00046     const double* dstPos = dest->as<KoulesStateSpace::StateType>()->values;
00047     double stepSize = si_->getPropagationStepSize();
00048     unsigned int steps = propagateMax_ ? si_->getMaxControlDuration() :
00049         cs_.sampleStepCount(si_->getMinControlDuration(), si_->getMaxControlDuration());
00050 
00051     cs_.steer(control, source, dstPos[0], dstPos[1]);
00052     // perform the first step of propagation
00053     statePropagator_->propagate(source, control, stepSize, dest);
00054     // if we reached the goal, we're done
00055     if (goal_->isSatisfied(dest))
00056         return 1;
00057     // if we found a valid state after one step, we can go on
00058     else if (si_->isValid(dest))
00059     {
00060         ob::State *temp1 = dest, *temp2 = si_->allocState(), *toDelete = temp2;
00061         unsigned int r = steps;
00062         for (unsigned int i = 1 ; i < steps ; ++i)
00063         {
00064             statePropagator_->propagate(temp1, control, stepSize, temp2);
00065             if (goal_->isSatisfied(dest))
00066             {
00067                 si_->copyState(dest, temp2);
00068                 si_->freeState(toDelete);
00069                 return i + 1;
00070             }
00071             else if (si_->isValid(temp2))
00072                 std::swap(temp1, temp2);
00073             else
00074             {
00075                 // the last valid state is temp1;
00076                 r = i;
00077                 break;
00078             }
00079         }
00080         // if we finished the for-loop without finding an invalid state, the last valid state is temp1
00081         // make sure dest contains that information
00082         if (dest != temp1)
00083             si_->copyState(dest, temp1);
00084         si_->freeState(toDelete);
00085         return r;
00086     }
00087     // if the first propagation step produced an invalid step, return 0 steps
00088     // the last valid state is the starting one (assumed to be valid)
00089     else
00090     {
00091         if (dest != source)
00092             si_->copyState(dest, source);
00093         return 0;
00094     }
00095 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines