Merge pull request #24 from joshhting/master

Implemented adaptive stepsize control

src/2dplane/2dplane.hpp

@@ -15,7 +15,7 @@
  * trying to find a path to
  *
  * @param step The fixed step size that the tree uses.  This is the
- * maximum distance between nodes.
+ * maximum distance between nodes unless adaptive stepsize control is utilized.
  *
  * @return An RRT::Tree with its callbacks and parameters configured.
  * You'll probably want to override the transitionValidator callback

src/2dplane/GridStateSpace.cpp

@@ -6,6 +6,8 @@
 using namespace Eigen;
 using namespace std;
 
+#include <iostream>
+
 
 GridStateSpace::GridStateSpace(float width, float height, int discretizedWidth, int discretizedHeight):
     PlaneStateSpace(width, height),
@@ -16,6 +18,29 @@ bool GridStateSpace::stateValid(const Vector2f &pt) const {
     return PlaneStateSpace::stateValid(pt) && !_obstacleGrid.obstacleAt(_obstacleGrid.gridSquareForLocation(pt));
 }
 
+Vector2f GridStateSpace::intermediateState(const Vector2f &source, const Vector2f &target, float minStepSize, float maxStepSize) const {
+    bool debug;
+
+    Vector2f delta = target - source;
+    delta = delta / delta.norm();   //  unit vector
+    float dist = _obstacleGrid.nearestObstacleDist(source, maxStepSize * 2);
+
+
+    float stepSize = (dist / maxStepSize) * minStepSize; // scale based on how far we are from obstacles
+    if (stepSize > maxStepSize) stepSize = maxStepSize;
+    if (stepSize < minStepSize) stepSize = minStepSize;
+    if (debug) {
+        cout << "ASC intermediateState" << endl;
+        cout << "  stepsize: " << minStepSize << endl;
+        cout << "  nearest obs dist: " << dist << endl;
+        cout << "  maximum stepsize: " << maxStepSize << endl;
+        cout << "  new step: " << stepSize << endl;
+    }
+
+    Vector2f val = source + delta * stepSize;
+    return val;
+}
+
 bool GridStateSpace::transitionValid(const Vector2f &from, const Vector2f &to) const {
     //  make sure we're within bounds
     if (!stateValid(to)) return false;

src/2dplane/GridStateSpace.hpp

@@ -19,10 +19,11 @@ class GridStateSpace : public PlaneStateSpace {
     bool stateValid(const Eigen::Vector2f &pt) const;
     bool transitionValid(const Eigen::Vector2f &from, const Eigen::Vector2f &to) const;
 
+    Eigen::Vector2f intermediateState(const Eigen::Vector2f &source, const Eigen::Vector2f &target, float minStepSize, float maxStepSize) const;
+
     const ObstacleGrid &obstacleGrid() const;
     ObstacleGrid &obstacleGrid();
 
-
 private:
     ObstacleGrid _obstacleGrid;
 };

src/2dplane/ObstacleGrid.cpp

@@ -1,7 +1,9 @@
 #include "ObstacleGrid.hpp"
 #include <stdlib.h>
+#include <iostream>
 
 using namespace Eigen;
+using namespace std;
 
 
 ObstacleGrid::ObstacleGrid(float width, float height, int discretizedWidth, int discretizedHeight) {
@@ -24,6 +26,36 @@ Vector2i ObstacleGrid::gridSquareForLocation(const Vector2f &loc) const {
                     loc.y() / height() * discretizedHeight());
 }
 
+float ObstacleGrid::nearestObstacleDist(const Vector2f &state, float maxDist) const {
+    //x and y are the indices of the cell that state is located in
+    float x = (state.x() / (_width / _discretizedWidth));
+    float y = (state.y() / (_height / _discretizedHeight));
+    int xSearchRad = maxDist * _discretizedWidth / _width;
+    int ySearchRad = maxDist * _discretizedHeight / _height;
+    //here we loop through the cells around (x,y) to find the minimum distance of the point to the nearest obstacle
+    for (int i = x - xSearchRad; i <= x + xSearchRad; i++) {
+        for (int j = y - ySearchRad; j <= y + ySearchRad; j++) {
+            bool obs = obstacleAt(i, j);
+            if (obs) {
+                float xDist = (x-i)*_width / _discretizedWidth;
+                float yDist = (y-j)*_height / _discretizedHeight;
+                float dist = sqrtf(powf(xDist, 2) + powf(yDist, 2));
+                if (dist < maxDist) {
+                    maxDist = dist;
+                }
+            }
+        }
+    }
+
+    // the boundaries of the grid count as obstacles
+    maxDist = std::min(maxDist, state.x());               // left boundary
+    maxDist = std::min(maxDist, width() - state.x());     // right boundary
+    maxDist = std::min(maxDist, state.y());               // top boundary
+    maxDist = std::min(maxDist, height() - state.y());    // bottom boundary
+
+    return maxDist;
+}
+
 void ObstacleGrid::clear() {
     for (int x = 0; x < discretizedWidth(); x++) {
         for (int y = 0; y < discretizedHeight(); y++) {

src/2dplane/ObstacleGrid.hpp

@@ -14,6 +14,14 @@ class ObstacleGrid {
 
     Eigen::Vector2i gridSquareForLocation(const Eigen::Vector2f &loc) const;
 
+    /** 
+     * Finds the distance from state to its neareset obstacle. Only searches up to maxDist around
+     * state so as to not waste time checking far away and irrelevant obstacles.
+     *
+     * @param state The location to search with respect to for the nearest obstacle dist
+     * @param maxDist The maximum vertical and horizontal distance from state to search for obstacles
+     */
+    float nearestObstacleDist(const Eigen::Vector2f &state, float maxDist) const;
     void clear();
     bool &obstacleAt(int x, int y);
     bool obstacleAt(int x, int y) const;

src/2dplane/PlaneStateSpace.cpp

@@ -39,3 +39,4 @@ float PlaneStateSpace::width() const {
 float PlaneStateSpace::height() const {
     return _height;
 }
+

src/BiRRT.hpp

@@ -36,6 +36,14 @@ namespace RRT
             return _goalTree;
         }
 
+        bool isASCEnabled() const {
+            return _startTree.isASCEnabled();
+        }
+        void setASCEnabled(bool checked) {
+            _startTree.setASCEnabled(checked);
+            _goalTree.setASCEnabled(checked);
+        }
+
         float goalBias() const {
             return _startTree.goalBias();
         }
@@ -76,6 +84,14 @@ namespace RRT
             _goalTree.setStepSize(stepSize);
         }
 
+        float maxStepSize() const {
+            return _startTree.maxStepSize();
+        }
+        void setMaxStepSize(float stepSize) {
+            _startTree.setMaxStepSize(stepSize);
+            _goalTree.setMaxStepSize(stepSize);
+        }
+
         float goalMaxDist() const {
             return _startTree.goalMaxDist();
         }

src/StateSpace.hpp

@@ -3,7 +3,7 @@
 
 /**
  * A state space represents the set of possible states for a planning problem.
- * This includes the obstacles that may bee present and what state transitions are valid.
+ * This includes the obstacles that may be present and what state transitions are valid.
  * This class is abstract and must be subclassed in order to provide actual functionality.
  */
 template<typename T>
@@ -27,6 +27,18 @@ class StateSpace {
      */
     virtual T intermediateState(const T &source, const T &target, float stepSize) const = 0;
 
+    /**
+     * An overloaded version designed for use in adaptive stepsize control.
+     *
+     * @param source The node in the tree to extend from
+     * @param target The point in the space to extend to
+     * @param minStepSize The minimum allowable stepsize the intermediate state will be extended from source
+     * @param maxStepSize The maximum allowable stepsize the intermediate state will be extended from source
+     *
+     * @return A state in the direction of @target from @source.state()
+     */
+    virtual T intermediateState(const T &source, const T &target, float minStepSize, float maxStepSize) const = 0;
+
     /**
      * @brief Calculate the distance between two states
      * 
@@ -56,4 +68,8 @@ class StateSpace {
      * @return A boolean indicating validity
      */
     virtual bool transitionValid(const T &from, const T &to) const = 0;
+
+protected:
+    float _minStepSize;
+    float _maxStepSize;
 };

src/Tree.hpp

@@ -7,12 +7,12 @@
 #include <functional>
 #include <stdexcept>
 #include <stdlib.h>
-
+#include <iostream>
 
 namespace RRT
 {
     /**
-     * Base class for an rrt tree node
+     * Base class for an RRT tree node.
      *
      * @param T The datatype representing the state in the space the RRT
      * will be searching.
@@ -23,7 +23,7 @@ namespace RRT
         Node(const T &state, Node<T> *parent = nullptr) {
             _parent = parent;
             _state = state;
-            
+
             if (_parent) {
                 _parent->_children.push_back(this);
             }
@@ -72,6 +72,11 @@ namespace RRT
      * placed in callbacks (C++ lambdas), which must be supplied by the
      * user of this class.
      *
+     * If adaptive stepsize control (ASC) is enabled, then the stepsize for extending new nodes from 
+     * the tree will be dynamically updated depending on how close the nearest obstacle is. If there are
+     * no nearby obstacles, then the stepsize will be extended in order to safely cover more ground. If
+     * there are nearby obstacles, then the stepsize will shrink so that the RRT can take more precise steps.
+     *
      * USAGE:
      * 1) Create a new Tree with the appropriate StateSpace
      *    RRT::Tree<My2dPoint> tree(stateSpace);
@@ -80,7 +85,10 @@ namespace RRT
      *    tree->setStartState(s);
      *    tree->setGoalState(g);
      *
-     * 3) Run the RRT algorithm!  This can be done in one of two ways:
+     * 3) (Optional) If adaptive stepsize control is enabled:
+     *    _stateSpace->setMaxStepSize sets the maximum stepsize the tree can take for any step.
+     *
+     * 4) Run the RRT algorithm!  This can be done in one of two ways:
      *    Option 1) Call the run() method - it will grow the tree
      *              until it finds a solution or runs out of iterations.
      *
@@ -89,7 +97,7 @@ namespace RRT
      *    Either way works fine, just choose whatever works best for your
      *    application.
      *
-     * 4) Use getPath() to get the series of states that make up the solution
+     * 5) Use getPath() to get the series of states that make up the solution
      *
      * @param T The type that represents a state within the state-space that
      * the tree is searching.  This could be a 2D Point or something else,
@@ -103,7 +111,9 @@ namespace RRT
 
             //  default values
             setStepSize(0.1);
+            setMaxStepSize(5);
             setMaxIterations(1000);
+            setASCEnabled(false);
             setGoalBias(0);
             setWaypointBias(0);
             setGoalMaxDist(0.1);
@@ -134,6 +144,16 @@ namespace RRT
         }
 
 
+        /**
+         * Whether or not the tree is to run with adaptive stepsize control.
+         */
+        bool isASCEnabled() const {
+            return _isASCEnabled;
+        }
+        void setASCEnabled(bool checked) {
+            _isASCEnabled = checked;
+        }
+
         /**
          * @brief The chance we extend towards the goal rather than a random point.
          * @details At each iteration of the RRT algorithm, we extend() towards a particular state.  The goalBias
@@ -187,6 +207,14 @@ namespace RRT
             _stepSize = stepSize;
         }
 
+        /// Max step size used in ASC
+        float maxStepSize() const {
+            return _maxStepSize;
+        }
+        void setMaxStepSize(float maxStep) {
+            _maxStepSize = maxStep;
+        }
+
 
         /**
          * @brief How close we have to get to the goal in order to consider it reached.
@@ -279,22 +307,33 @@ namespace RRT
          * Grow the tree in the direction of @state
          *
          * @return the new tree Node (may be nullptr if we hit Obstacles)
+         * @param target The point to extend the tree to
          * @param source The Node to connect from.  If source == nullptr, then
          *             the closest tree point is used
          */
         virtual Node<T> *extend(const T &target, Node<T> *source = nullptr) {
             //  if we weren't given a source point, try to find a close node
             if (!source) {
-                source = nearest(target);
+                source = nearest(target, nullptr);
                 if (!source) {
                     return nullptr;
                 }
             }
-            
+
             //  Get a state that's in the direction of @target from @source.
             //  This should take a step in that direction, but not go all the
             //  way unless the they're really close together.
-            T intermediateState = _stateSpace->intermediateState(source->state(), target, stepSize());
+            T intermediateState;
+            if (_isASCEnabled) {
+                intermediateState = _stateSpace->intermediateState(
+                    source->state(),
+                    target,
+                    stepSize(),
+                    maxStepSize()
+                );
+            } else {
+                intermediateState = _stateSpace->intermediateState(source->state(), target, stepSize());
+            }
 
             //  Make sure there's actually a direct path from @source to
             //  @intermediateState.  If not, abort
@@ -413,6 +452,8 @@ namespace RRT
 
         int _maxIterations;
 
+        bool _isASCEnabled;
+
         float _goalBias;
 
         /// used for Extended RRTs where growth is biased towards waypoints from previously grown tree
@@ -422,6 +463,7 @@ namespace RRT
         float _goalMaxDist;
 
         float _stepSize;
+        float _maxStepSize;
 
         std::shared_ptr<StateSpace<T>> _stateSpace;
     };

src/TreeTest.cpp

@@ -66,3 +66,35 @@ TEST(Tree, GetPath) {
 	tree->getPath(path, tree->lastNode(), false);
 	ASSERT_TRUE(path.size() > 1);
 }
+
+TEST(Tree, ASC) {
+	//test adaptive stepsize control
+	Tree<Vector2f> *tree = TreeFor2dPlane(
+		make_shared<GridStateSpace>(50, 50, 50, 50),
+		Vector2f(40, 40),	//	goal point
+		5);					//	step size
+
+	//	give it plenty of iterations so it's not likely to fail
+	const int maxIterations = 10000;
+	tree->setMaxIterations(maxIterations);
+	tree->setGoalMaxDist(5);
+	tree->setMaxStepSize(10);
+	tree->setASCEnabled(true);
+
+	tree->setStartState(Vector2f(10, 10));
+	bool success = tree->run();	//	run with the given starting point
+	ASSERT_TRUE(success);
+
+	vector<Vector2f> path;
+	tree->getPath(path, tree->lastNode(), true);
+
+	// Check to see if the nodes in the tree have uniform stepsize or varied. Stepsizes should vary
+	bool varied = false;
+	for (int i = 1; !varied && i < path.size() - 2; i++) {
+		Vector2f pathA = path[i] - path[i - 1];
+		Vector2f pathB = path[i] - path[i + 1];
+		float n = pathA.norm() / pathB.norm();
+		if (n < 0.99 || n > 1.01) varied = true;
+	}
+	ASSERT_TRUE(varied);
+}