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(goal->state, 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->states.size() ; ++i)
00130             if (!fixInvalidInputState(goals->states[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->state);
00146 
00147     GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
00148     if (goals)
00149         for (unsigned int i = 0 ; i < goals->states.size() ; ++i)
00150             states.push_back(goals->states[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->states.push_back(sic->cloneState(startStates_[startIndex]));
00163             pc->states.push_back(sic->cloneState(startStates_[startIndex]));
00164             pc->controls.push_back(sic->allocControl());
00165             sic->nullControl(pc->controls.back());
00166             pc->controlDurations.push_back(0);
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->states.push_back(sic->cloneState(start));
00189                                 pc->states.push_back(sic->cloneState(result2));
00190                                 pc->controls.push_back(sic->cloneControl(nc));
00191                                 pc->controlDurations.push_back(i + 1);
00192                                 path.reset(pc);
00193                                 break;
00194                             }
00195                             std::swap(result1, result2);
00196                         }
00197                 }
00198             }
00199             sic->freeState(result1);
00200             sic->freeState(result2);
00201             sic->freeControl(nc);
00202         }
00203     }
00204     else
00205     {
00206         std::vector<const State*> states;
00207         GoalState *goal = dynamic_cast<GoalState*>(goal_.get());
00208         if (goal)
00209             if (si_->isValid(goal->state) && si_->satisfiesBounds(goal->state))
00210                 states.push_back(goal->state);
00211         GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get());
00212         if (goals)
00213             for (unsigned int i = 0 ; i < goals->states.size() ; ++i)
00214                 if (si_->isValid(goals->states[i]) && si_->satisfiesBounds(goals->states[i]))
00215                     states.push_back(goals->states[i]);
00216 
00217         if (states.empty())
00218         {
00219             unsigned int startIndex;
00220             if (isTrivial(&startIndex))
00221             {
00222                 geometric::PathGeometric *pg = new geometric::PathGeometric(si_);
00223                 pg->states.push_back(si_->cloneState(startStates_[startIndex]));
00224                 pg->states.push_back(si_->cloneState(startStates_[startIndex]));
00225                 path.reset(pg);
00226             }
00227         }
00228         else
00229         {
00230             for (unsigned int i = 0 ; i < startStates_.size() && !path ; ++i)
00231             {
00232                 const State *start = startStates_[i];
00233                 if (start && si_->isValid(start) && si_->satisfiesBounds(start))
00234                 {
00235                     for (unsigned int j = 0 ; j < states.size() && !path ; ++j)
00236                         if (si_->checkMotion(start, states[j]))
00237                         {
00238                             geometric::PathGeometric *pg = new geometric::PathGeometric(si_);
00239                             pg->states.push_back(si_->cloneState(start));
00240                             pg->states.push_back(si_->cloneState(states[j]));
00241                             path.reset(pg);
00242                             break;
00243                         }
00244                 }
00245             }
00246         }
00247     }
00248 
00249     return path;
00250 }
00251 
00252 bool ompl::base::ProblemDefinition::isTrivial(unsigned int *startIndex, double *distance) const
00253 {
00254     if (!goal_)
00255     {
00256         msg_.error("Goal undefined");
00257         return false;
00258     }
00259 
00260     for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00261     {
00262         const State *start = startStates_[i];
00263         if (start && si_->isValid(start) && si_->satisfiesBounds(start))
00264         {
00265             double dist;
00266             if (goal_->isSatisfied(start, &dist))
00267             {
00268                 if (startIndex)
00269                     *startIndex = i;
00270                 if (distance)
00271                     *distance = dist;
00272                 return true;
00273             }
00274         }
00275         else
00276         {
00277             msg_.error("Initial state is in collision!");
00278         }
00279     }
00280 
00281     return false;
00282 }
00283 
00284 void ompl::base::ProblemDefinition::print(std::ostream &out) const
00285 {
00286     out << "Start states:" << std::endl;
00287     for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00288         si_->printState(startStates_[i], out);
00289     if (goal_)
00290         goal_->print(out);
00291     else
00292         out << "Goal = NULL" << std::endl;
00293 }