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/DiscreteMotionValidator.h"
00038 #include "ompl/util/Exception.h"
00039 #include <queue>
00040
00041 void ompl::base::DiscreteMotionValidator::defaultSettings(void)
00042 {
00043 stateSpace_ = si_->getStateSpace().get();
00044 if (!stateSpace_)
00045 throw Exception("No state space for motion validator");
00046 }
00047
00048 bool ompl::base::DiscreteMotionValidator::checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
00049 {
00050
00051
00052 bool result = true;
00053 int nd = stateSpace_->validSegmentCount(s1, s2);
00054
00055 if (nd > 1)
00056 {
00057
00058 State *test = si_->allocState();
00059
00060 for (int j = 1 ; j < nd ; ++j)
00061 {
00062 stateSpace_->interpolate(s1, s2, (double)j / (double)nd, test);
00063 if (!si_->isValid(test))
00064 {
00065 lastValid.second = (double)(j - 1) / (double)nd;
00066 if (lastValid.first)
00067 stateSpace_->interpolate(s1, s2, lastValid.second, lastValid.first);
00068 result = false;
00069 break;
00070 }
00071 }
00072 si_->freeState(test);
00073 }
00074
00075 if (result)
00076 if (!si_->isValid(s2))
00077 {
00078 lastValid.second = (double)(nd - 1) / (double)nd;
00079 if (lastValid.first)
00080 stateSpace_->interpolate(s1, s2, lastValid.second, lastValid.first);
00081 result = false;
00082 }
00083
00084 if (result)
00085 valid_++;
00086 else
00087 invalid_++;
00088
00089 return result;
00090 }
00091
00092 bool ompl::base::DiscreteMotionValidator::checkMotion(const State *s1, const State *s2) const
00093 {
00094
00095 if (!si_->isValid(s2))
00096 {
00097 invalid_++;
00098 return false;
00099 }
00100
00101 bool result = true;
00102 int nd = stateSpace_->validSegmentCount(s1, s2);
00103
00104
00105 std::queue< std::pair<int, int> > pos;
00106 if (nd >= 2)
00107 {
00108 pos.push(std::make_pair(1, nd - 1));
00109
00110
00111 State *test = si_->allocState();
00112
00113
00114 while (!pos.empty())
00115 {
00116 std::pair<int, int> x = pos.front();
00117
00118 int mid = (x.first + x.second) / 2;
00119 stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, test);
00120
00121 if (!si_->isValid(test))
00122 {
00123 result = false;
00124 break;
00125 }
00126
00127 pos.pop();
00128
00129 if (x.first < mid)
00130 pos.push(std::make_pair(x.first, mid - 1));
00131 if (x.second > mid)
00132 pos.push(std::make_pair(mid + 1, x.second));
00133 }
00134
00135 si_->freeState(test);
00136 }
00137
00138 if (result)
00139 valid_++;
00140 else
00141 invalid_++;
00142
00143 return result;
00144 }