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(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 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 }