Add BLITstar

Author: yi213-robotic

demos/OptimalPlanning.cpp

@@ -43,6 +43,7 @@
 #include "ompl/util/Console.h"
 
 // The supported optimal planners, in alphabetical order
+#include <ompl/geometric/planners/lazyinformedtrees/BLITstar.h>
 #include <ompl/geometric/planners/informedtrees/AITstar.h>
 #include <ompl/geometric/planners/informedtrees/BITstar.h>
 #include <ompl/geometric/planners/informedtrees/EITstar.h>
@@ -74,6 +75,7 @@ enum optimalPlanner
     PLANNER_AITSTAR,
     PLANNER_BFMTSTAR,
     PLANNER_BITSTAR,
+    PLANNER_BLITSTAR,
     PLANNER_CFOREST,
     PLANNER_EITSTAR,
     PLANNER_EIRMSTAR,
@@ -129,6 +131,7 @@ class ValidityChecker : public ob::StateValidityChecker
 
         // Distance formula between two points, offset by the circle's
         // radius
+        return ((x >= 1.4 && x <= 1.5 && y >= 0.6 && y <=1.4) || ( y >= 1.3 && y <= 1.4 && x>= 0.6 && x<= 1.4)) ? 0.0 : 0.1;
         return sqrt((x - 0.5) * (x - 0.5) + (y - 0.5) * (y - 0.5)) - 0.25;
     }
 };
@@ -164,6 +167,11 @@ ob::PlannerPtr allocatePlanner(ob::SpaceInformationPtr si, optimalPlanner planne
             return std::make_shared<og::BITstar>(si);
             break;
         }
+        case PLANNER_BLITSTAR:
+        {
+            return std::make_shared<og::BLITstar>(si);
+            break;
+        }
         case PLANNER_CFOREST:
         {
             return std::make_shared<og::CForest>(si);
@@ -243,11 +251,16 @@ void plan(double runTime, optimalPlanner plannerType, planningObjective objectiv
     auto space(std::make_shared<ob::RealVectorStateSpace>(2));
 
     // Set the bounds of space to be in [0,1].
-    space->setBounds(0.0, 1.0);
+    //space->setBounds(0.0, 1.0);
 
+    ob::RealVectorBounds bounds(2); 
+    bounds.setLow(0, 0);  // Lower bound for x-dimension
+    bounds.setHigh(0, 2);  // Upper bound for x-dimension
+    bounds.setLow(1, 0);  // Lower bound for y-dimension
+    bounds.setHigh(1, 2);  // Upper bound for y-dimension 
     // Construct a space information instance for this state space
+    space->setBounds(bounds);
     auto si(std::make_shared<ob::SpaceInformation>(space));
-
     // Set the object used to check which states in the space are valid
     si->setStateValidityChecker(std::make_shared<ValidityChecker>(si));
 
@@ -262,8 +275,8 @@ void plan(double runTime, optimalPlanner plannerType, planningObjective objectiv
     // Set our robot's goal state to be the top-right corner of the
     // environment, or (1,1).
     ob::ScopedState<> goal(space);
-    goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.0;
-    goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.0;
+    goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = 1.8;
+    goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = 1.8;
 
     // Create a problem instance
     auto pdef(std::make_shared<ob::ProblemDefinition>(si));
@@ -438,10 +451,10 @@ bool argParse(int argc, char **argv, double *runTimePtr, optimalPlanner *planner
     desc.add_options()("help,h", "produce help message")(
         "runtime,t", bpo::value<double>()->default_value(1.0),
         "(Optional) Specify the runtime in seconds. Defaults to 1 and "
-        "must be greater than 0.")("planner,p", bpo::value<std::string>()->default_value("RRTstar"),
+        "must be greater than 0.")("planner,p", bpo::value<std::string>()->default_value("BLITstar"),
                                    "(Optional) Specify the optimal planner to use, defaults to RRTstar if not given. "
                                    "Valid options are AITstar, "
-                                   "BFMTstar, BITstar, CForest, EITstar, EIRMstar, FMTstar, InformedRRTstar, PRMstar, RRTstar, "
+                                   "BFMTstar, BITstar, BLITstar, CForest, EITstar, EIRMstar, FMTstar, InformedRRTstar, PRMstar, RRTstar, "
                                    "and SORRTstar.")  // Alphabetical order
         ("objective,o", bpo::value<std::string>()->default_value("PathLength"),
          "(Optional) Specify the optimization objective, defaults to PathLength if not given. Valid options are "
@@ -509,6 +522,10 @@ bool argParse(int argc, char **argv, double *runTimePtr, optimalPlanner *planner
     {
         *plannerPtr = PLANNER_BITSTAR;
     }
+    else if (boost::iequals("BLITstar", plannerStr))
+    {
+        *plannerPtr = PLANNER_BLITSTAR;
+    }
     else if (boost::iequals("CForest", plannerStr))
     {
         *plannerPtr = PLANNER_CFOREST;

py-bindings/headers_geometric.txt

@@ -33,6 +33,7 @@ src/ompl/geometric/planners/rrt/InformedRRTstar.h
 src/ompl/geometric/planners/rrt/SORRTstar.h
 src/ompl/geometric/planners/rrt/STRRTstar.h
 src/ompl/geometric/planners/informedtrees/BITstar.h
+src/ompl/geometric/planners/lazyinformedtrees/BLITstar.h
 src/ompl/geometric/planners/informedtrees/ABITstar.h
 src/ompl/geometric/planners/informedtrees/AITstar.h
 src/ompl/geometric/planners/informedtrees/EITstar.h

src/ompl/base/OptimizationObjective.h

@@ -101,6 +101,8 @@ namespace ompl
             /** \brief Check whether the the cost \e c1 is considered better than the cost \e c2. By default, this
              * returns true if if c1 is less than c2. */
             virtual bool isCostBetterThan(Cost c1, Cost c2) const;
+            
+            virtual bool isCostLargerThan(Cost c1, Cost c2) const;
 
             /** \brief Compare whether cost \e c1 and cost \e c2 are equivalent. By default defined as
              * !isCostBetterThan(c1, c2) && !isCostBetterThan(c2, c1), as if c1 is not better than c2, and c2 is not

src/ompl/base/src/OptimizationObjective.cpp

@@ -69,7 +69,12 @@ void ompl::base::OptimizationObjective::setCostThreshold(Cost c)
 
 bool ompl::base::OptimizationObjective::isCostBetterThan(Cost c1, Cost c2) const
 {
-    return c1.value() < c2.value();
+    return c1.value() + 0.000001 < c2.value();
+}
+
+bool ompl::base::OptimizationObjective::isCostLargerThan(Cost c1, Cost c2) const
+{
+    return c1.value() > c2.value()+ 0.000001;
 }
 
 bool ompl::base::OptimizationObjective::isCostEquivalentTo(Cost c1, Cost c2) const