00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00113 for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00114 if (!fixInvalidInputState(startStates_[i], distStart, true, attempts))
00115 result = false;
00116
00117
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
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 }