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 
00041 #include <sstream>
00042 
00043 void ompl::base::ProblemDefinition::setStartAndGoalStates(const State *start, const State *goal, const double threshold)
00044 {
00045     clearStartStates();
00046     addStartState(start);
00047     setGoalState(goal, threshold);
00048 }
00049 
00050 void ompl::base::ProblemDefinition::setGoalState(const State *goal, const double threshold)
00051 {
00052     clearGoal();
00053     GoalState *gs = new GoalState(si_);
00054     gs->setState(goal);
00055     gs->setThreshold(threshold);
00056     setGoal(GoalPtr(gs));
00057 }
00058 
00059 bool ompl::base::ProblemDefinition::hasStartState(const State *state, unsigned int *startIndex)
00060 {
00061     for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00062         if (si_->equalStates(state, startStates_[i]))
00063         {
00064             if (startIndex)
00065                 *startIndex = i;
00066             return true;
00067         }
00068     return false;
00069 }
00070 
00071 bool ompl::base::ProblemDefinition::fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts)
00072 {
00073     bool result = false;
00074 
00075     bool b = si_->satisfiesBounds(state);
00076     bool v = false;
00077     if (b)
00078     {
00079         v = si_->isValid(state);
00080         if (!v)
00081             msg_.debug("%s state is not valid", start ? "Start" : "Goal");
00082     }
00083     else
00084         msg_.debug("%s state is not within space bounds", start ? "Start" : "Goal");
00085 
00086     if (!b || !v)
00087     {
00088         std::stringstream ss;
00089         si_->printState(state, ss);
00090         ss << " within distance " << dist;
00091         msg_.debug("Attempting to fix %s state %s", start ? "start" : "goal", ss.str().c_str());
00092 
00093         State *temp = si_->allocState();
00094         if (si_->searchValidNearby(temp, state, dist, attempts))
00095         {
00096             si_->copyState(state, temp);
00097             result = true;
00098         }
00099         else
00100             msg_.warn("Unable to fix %s state", start ? "start" : "goal");
00101         si_->freeState(temp);
00102     }
00103 
00104     return result;
00105 }
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 bool ompl::base::ProblemDefinition::isTrivial(unsigned int *startIndex, double *distance) const
00154 {
00155     if (!goal_)
00156     {
00157         msg_.error("Goal undefined");
00158         return false;
00159     }
00160 
00161     for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00162     {
00163         const State *start = startStates_[i];
00164         if (start && si_->isValid(start) && si_->satisfiesBounds(start))
00165         {
00166             double dist;
00167             if (goal_->isSatisfied(start, &dist))
00168             {
00169                 if (startIndex)
00170                     *startIndex = i;
00171                 if (distance)
00172                     *distance = dist;
00173                 return true;
00174             }
00175         }
00176         else
00177         {
00178             msg_.error("Initial state is in collision!");
00179         }
00180     }
00181 
00182     return false;
00183 }
00184 
00185 void ompl::base::ProblemDefinition::print(std::ostream &out) const
00186 {
00187     out << "Start states:" << std::endl;
00188     for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00189         si_->printState(startStates_[i], out);
00190     if (goal_)
00191         goal_->print(out);
00192     else
00193         out << "Goal = NULL" << std::endl;
00194 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator