All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
ProblemDefinition.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/base/ProblemDefinition.h"
00038 #include "ompl/base/GoalState.h"
00039 #include "ompl/base/GoalStates.h"
00040 #include "ompl/control/SpaceInformation.h"
00041 #include "ompl/control/PathControl.h"
00042 #include <sstream>
00043 
00044 void ompl::base::ProblemDefinition::setStartAndGoalStates(const State *start, const State *goal, const double threshold)
00045 {
00046     clearStartStates();
00047     addStartState(start);
00048     setGoalState(goal, threshold);
00049 }
00050 
00051 void ompl::base::ProblemDefinition::setGoalState(const State *goal, const double threshold)
00052 {
00053     clearGoal();
00054     GoalState *gs = new GoalState(si_);
00055     gs->setState(goal);
00056     gs->setThreshold(threshold);
00057     setGoal(GoalPtr(gs));
00058 }
00059 
00060 bool ompl::base::ProblemDefinition::hasStartState(const State *state, unsigned int *startIndex)
00061 {
00062     for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00063         if (si_->equalStates(state, startStates_[i]))
00064         {
00065             if (startIndex)
00066                 *startIndex = i;
00067             return true;
00068         }
00069     return false;
00070 }
00071 
00072 bool ompl::base::ProblemDefinition::fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
00073 {
00074     bool result = false;
00075 
00076     bool b = si_->satisfiesBounds(state);
00077     bool v = false;
00078     if (b)
00079     {
00080         v = si_->isValid(state);
00081         if (!v)
00082             msg_.debug("%s state is not valid", start ? "Start" : "Goal");
00083     }
00084     else
00085         msg_.debug("%s state is not within space bounds", start ? "Start" : "Goal");
00086 
00087     if (!b || !v)
00088     {
00089         std::stringstream ss;
00090         si_->printState(state, ss);
00091         ss << " within distance " << dist;
00092         msg_.debug("Attempting to fix %s state %s", start ? "start" : "goal", ss.str().c_str());
00093 
00094         State *temp = si_->allocState();
00095         if (si_->searchValidNearby(temp, state, dist, attempts))
00096         {
00097             si_->copyState(state, temp);
00098             result = true;
00099         }
00100         else
00101             msg_.warn("Unable to fix %s state", start ? "start" : "goal");
00102         si_->freeState(temp);
00103     }
00104 
00105     return result;
00106 }
00107 
00108 bool ompl::base::ProblemDefinition::fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts)
00109 {
00110     bool result = true;
00111 
00112     // fix start states
00113     for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00114         if (!fixInvalidInputState(startStates_[i], distStart, true, attempts))
00115             result = false;
00116 
00117     // fix goal state
00118     GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
00119     if (goal)
00120     {
00121         if (!fixInvalidInputState(const_cast<State*>(goal->getState()), distGoal, false, attempts))
00122             result = false;
00123     }
00124 
00125     // fix goal state
00126     GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
00127     if (goals)
00128     {
00129         for (unsigned int i = 0; i < goals->getStateCount(); ++i)
00130             if (!fixInvalidInputState(const_cast<State*>(goals->getState(i)), distGoal, false, attempts))
00131                 result = false;
00132     }
00133 
00134     return result;
00135 }
00136 
00137 void ompl::base::ProblemDefinition::getInputStates(std::vector<const State*> &states) const
00138 {
00139     states.clear();
00140     for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00141         states.push_back(startStates_[i]);
00142 
00143     GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
00144     if (goal)
00145         states.push_back(goal->getState());
00146 
00147     GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
00148     if (goals)
00149         for (unsigned int i = 0; i < goals->getStateCount(); ++i)
00150             states.push_back (goals->getState(i));
00151 }
00152 
00153 ompl::base::PathPtr ompl::base::ProblemDefinition::isStraightLinePathValid(void) const
00154 {
00155     PathPtr path;
00156     if (control::SpaceInformationPtr sic = boost::dynamic_pointer_cast<control::SpaceInformation, SpaceInformation>(si_))
00157     {
00158         unsigned int startIndex;
00159         if (isTrivial(&startIndex, NULL))
00160         {
00161             control::PathControl *pc = new control::PathControl(sic);
00162             pc->append(startStates_[startIndex]);
00163             control::Control *null = sic->allocControl();
00164             sic->nullControl(null);
00165             pc->append(startStates_[startIndex], null, 0.0);
00166             sic->freeControl(null);
00167             path.reset(pc);
00168         }
00169         else
00170         {
00171             control::Control *nc = sic->allocControl();
00172             State *result1 = sic->allocState();
00173             State *result2 = sic->allocState();
00174             sic->nullControl(nc);
00175 
00176             for (unsigned int k = 0 ; k < startStates_.size() && !path ; ++k)
00177             {
00178                 const State *start = startStates_[k];
00179                 if (start && si_->isValid(start) && si_->satisfiesBounds(start))
00180                 {
00181                     sic->copyState(result1, start);
00182                     for (unsigned int i = 0 ; i < sic->getMaxControlDuration() && !path ; ++i)
00183                         if (sic->propagateWhileValid(result1, nc, 1, result2))
00184                         {
00185                             if (goal_->isSatisfied(result2))
00186                             {
00187                                 control::PathControl *pc = new control::PathControl(sic);
00188                                 pc->append(start);
00189                                 pc->append(result2, nc, (i + 1) * sic->getPropagationStepSize());
00190                                 path.reset(pc);
00191                                 break;
00192                             }
00193                             std::swap(result1, result2);
00194                         }
00195                 }
00196             }
00197             sic->freeState(result1);
00198             sic->freeState(result2);
00199             sic->freeControl(nc);
00200         }
00201     }
00202     else
00203     {
00204         std::vector<const State*> states;
00205         GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
00206         if (goal)
00207             if (si_->isValid(goal->getState()) && si_->satisfiesBounds(goal->getState()))
00208                 states.push_back(goal->getState());
00209         GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
00210         if (goals)
00211             for (unsigned int i = 0; i < goals->getStateCount(); ++i)
00212                 if (si_->isValid(goals->getState(i)) && si_->satisfiesBounds(goals->getState(i)))
00213                     states.push_back(goals->getState(i));
00214 
00215         if (states.empty())
00216         {
00217             unsigned int startIndex;
00218             if (isTrivial(&startIndex))
00219             {
00220                 geometric::PathGeometric *pg = new geometric::PathGeometric(si_, startStates_[startIndex], startStates_[startIndex]);
00221                 path.reset(pg);
00222             }
00223         }
00224         else
00225         {
00226             for (unsigned int i = 0 ; i < startStates_.size() && !path ; ++i)
00227             {
00228                 const State *start = startStates_[i];
00229                 if (start && si_->isValid(start) && si_->satisfiesBounds(start))
00230                 {
00231                     for (unsigned int j = 0 ; j < states.size() && !path ; ++j)
00232                         if (si_->checkMotion(start, states[j]))
00233                         {
00234                             geometric::PathGeometric *pg = new geometric::PathGeometric(si_, start, states[j]);
00235                             path.reset(pg);
00236                             break;
00237                         }
00238                 }
00239             }
00240         }
00241     }
00242 
00243     return path;
00244 }
00245 
00246 bool ompl::base::ProblemDefinition::isTrivial(unsigned int *startIndex, double *distance) const
00247 {
00248     if (!goal_)
00249     {
00250         msg_.error("Goal undefined");
00251         return false;
00252     }
00253 
00254     for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00255     {
00256         const State *start = startStates_[i];
00257         if (start && si_->isValid(start) && si_->satisfiesBounds(start))
00258         {
00259             double dist;
00260             if (goal_->isSatisfied(start, &dist))
00261             {
00262                 if (startIndex)
00263                     *startIndex = i;
00264                 if (distance)
00265                     *distance = dist;
00266                 return true;
00267             }
00268         }
00269         else
00270         {
00271             msg_.error("Initial state is in collision!");
00272         }
00273     }
00274 
00275     return false;
00276 }
00277 
00278 void ompl::base::ProblemDefinition::print(std::ostream &out) const
00279 {
00280     out << "Start states:" << std::endl;
00281     for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00282         si_->printState(startStates_[i], out);
00283     if (goal_)
00284         goal_->print(out);
00285     else
00286         out << "Goal = NULL" << std::endl;
00287 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends