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 #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
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(const_cast<State*>(goal->getState()), 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->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 }