/** Create an optimization objective equivalent to the one returned by
getBalancedObjective1(), but use an alternate syntax.
*/
ob::OptimizationObjectivePtr getBalancedObjective2(const ob::SpaceInformationPtr& si)
{
ob::OptimizationObjectivePtr lengthObj(new ob::PathLengthOptimizationObjective(si));
ob::OptimizationObjectivePtr clearObj(new ClearanceObjective(si));
return 10.0*lengthObj + clearObj;
}
/** Create an optimization objective which attempts to optimize both
path length and clearance. We do this by defining our individual
objectives, then adding them to a MultiOptimizationObjective
object. This results in an optimization objective where path cost
is equivalent to adding up each of the individual objectives' path
costs.
When adding objectives, we can also optionally specify each
objective's weighting factor to signify how important it is in
optimal planning. If no weight is specified, the weight defaults to
1.0.
*/
ob::OptimizationObjectivePtr getBalancedObjective1(const ob::SpaceInformationPtr& si)
{
ob::OptimizationObjectivePtr lengthObj(new ob::PathLengthOptimizationObjective(si));
ob::OptimizationObjectivePtr clearObj(new ClearanceObjective(si));
ob::MultiOptimizationObjective* opt = new ob::MultiOptimizationObjective(si);
opt->addObjective(lengthObj, 10.0);
opt->addObjective(clearObj, 1.0);
return ob::OptimizationObjectivePtr(opt);
}
int main(int argc, char **argv)
{
if(argc!=3){
std::cout << "Usage: " << argv[0] << " maxDim nbRun" <<std::endl;
exit(0);
}
int maxDim = atoi(argv[1]);
int nbRun = atoi(argv[2]);
double maxTime=1000.0;
double threshold=1.025;
ompl::msg::noOutputHandler();
for(int p=0;p<9;++p){
switch(p){
case 0 : std::cout << "RRT*" << std::endl;break;
case 1 : std::cout << "RRT#" << std::endl;break;
case 2 : std::cout << "DRRT" << std::endl;break;
case 3 : std::cout << "RRT*RS" << std::endl;break;
case 4 : std::cout << "RRT#RS" << std::endl;break;
case 5 : std::cout << "DRRTRS" << std::endl;break;
case 6 : std::cout << "RRT*IS" << std::endl;break;
case 7 : std::cout << "RRT#IS" << std::endl;break;
case 8 : std::cout << "DRRTIS" << std::endl;break;
}
for(int dim=1;dim<=maxDim;++dim){
std::vector<double> times;
std::vector<int> iterations;
for(int r=0;r<nbRun;++r){
//Setup space
ompl::base::StateSpacePtr space(new ompl::base::RealVectorStateSpace(dim));
ompl::geometric::SimpleSetup ss(space);
space->as<ompl::base::RealVectorStateSpace>()->setBounds(-1,1);
//Setup Validity checker for nodes and edges
ss.setStateValidityChecker(ompl::base::StateValidityCheckerPtr(new ValidityChecker(ss.getSpaceInformation())));
ss.getSpaceInformation()->setMotionValidator(ompl::base::MotionValidatorPtr(new myMotionValidator(ss.getSpaceInformation())));
ss.getSpaceInformation()->setup();
//Define start and goal
ompl::base::ScopedState<> start(space), goal(space);
for(int i=0;i<dim;++i){
start[i]=-1;
goal[i]=1;
}
ss.setStartAndGoalStates(start, goal);
//Define Optimization function such that optimal cost is 1
double range=0.1*sqrt(dim);
ompl::base::OptimizationObjectivePtr lengthObj(new ompl::base::PathLengthOptimizationObjective(ss.getSpaceInformation()));
ompl::base::OptimizationObjectivePtr oop;
oop=(0.5/sqrt(dim))*lengthObj;
oop->setCostThreshold(ompl::base::Cost(threshold));
ss.setOptimizationObjective(oop);
ompl::base::PlannerPtr planner;
switch(p){
case 0 : planner=ompl::base::PlannerPtr(new ompl::geometric::RRTstar(ss.getSpaceInformation()));
planner->as<ompl::geometric::RRTstar>()->setRange(range);
break;
case 1 : planner=ompl::base::PlannerPtr(new ompl::geometric::RRTsharp(ss.getSpaceInformation()));
planner->as<ompl::geometric::RRTsharp>()->setRange(range);
break;
case 2 : planner=ompl::base::PlannerPtr(new ompl::geometric::DRRT(ss.getSpaceInformation()));
planner->as<ompl::geometric::DRRT>()->setRange(range);
break;
case 3 : planner=ompl::base::PlannerPtr(new ompl::geometric::RRTstar(ss.getSpaceInformation()));
planner->as<ompl::geometric::RRTstar>()->setRange(range);
planner->as<ompl::geometric::RRTstar>()->setSampleRejection(true);
break;
case 4 : planner=ompl::base::PlannerPtr(new ompl::geometric::RRTsharp(ss.getSpaceInformation()));
planner->as<ompl::geometric::RRTsharp>()->setRange(range);
planner->as<ompl::geometric::RRTsharp>()->setSampleRejection(true);
break;
case 5 : planner=ompl::base::PlannerPtr(new ompl::geometric::DRRT(ss.getSpaceInformation()));
planner->as<ompl::geometric::DRRT>()->setRange(range);
planner->as<ompl::geometric::DRRT>()->setSampleRejection(true);
break;
case 6 : planner=ompl::base::PlannerPtr(new ompl::geometric::RRTstar(ss.getSpaceInformation()));
planner->as<ompl::geometric::RRTstar>()->setRange(range);
planner->as<ompl::geometric::RRTstar>()->setInformedSampling(true);
break;
case 7 : planner=ompl::base::PlannerPtr(new ompl::geometric::RRTsharp(ss.getSpaceInformation()));
planner->as<ompl::geometric::RRTsharp>()->setRange(range);
planner->as<ompl::geometric::RRTsharp>()->setInformedSampling(true);
break;
case 8 : planner=ompl::base::PlannerPtr(new ompl::geometric::DRRT(ss.getSpaceInformation()));
planner->as<ompl::geometric::DRRT>()->setRange(range);
planner->as<ompl::geometric::DRRT>()->setInformedSampling(true);
break;
}
ss.setPlanner(planner);
ompl::time::point timeStart = ompl::time::now();
ss.solve(maxTime);
double timeUsed = ompl::time::seconds(ompl::time::now() - timeStart);
times.push_back(timeUsed);
if(timeUsed>maxTime)
break;
switch(p){
case 0 : iterations.push_back(planner->as<ompl::geometric::RRTstar>()->numIterations());
break;
case 1 : iterations.push_back(planner->as<ompl::geometric::RRTsharp>()->numIterations());
break;
case 2 : iterations.push_back(planner->as<ompl::geometric::DRRT>()->numIterations());
break;
case 3 : iterations.push_back(planner->as<ompl::geometric::RRTstar>()->numIterations());
break;
case 4 : iterations.push_back(planner->as<ompl::geometric::RRTsharp>()->numIterations());
break;
case 5 : iterations.push_back(planner->as<ompl::geometric::DRRT>()->numIterations());
break;
case 6 : iterations.push_back(planner->as<ompl::geometric::RRTstar>()->numIterations());
break;
case 7 : iterations.push_back(planner->as<ompl::geometric::RRTsharp>()->numIterations());
break;
case 8 : iterations.push_back(planner->as<ompl::geometric::DRRT>()->numIterations());
break;
}
}
if(times.empty() || times.back()>maxTime)
break;
// Compute average and standard deviation.
double sum = 0;
double sum_it = 0;
for (unsigned int i = 0; i < times.size(); ++ i) {
sum += times[i];
sum_it += iterations[i];
}
double average = double(sum) / times.size();
double it_avg = double(sum_it) / times.size();
double sum_sq_diff = 0.0 , std_dev = 0.0;
double sum_sq_diff_it = 0.0 , it_std = 0.0;
if(times.size()>1){
for (unsigned int i=0; i<times.size(); ++i) {
double diff = times[i] - average;
sum_sq_diff += diff*diff;
double diff_it = iterations[i] - it_avg;
sum_sq_diff_it += diff_it*diff_it;
}
std_dev = sqrt(sum_sq_diff / (times.size()-1));
it_std = sqrt(sum_sq_diff_it / (times.size()-1));
}
std::cout << dim << " " << average << " " << std_dev << " " << it_avg << " " << it_std << std::endl;
}
}
exit(0);
}
