randomBiasState

src/rrt/2dplane/PlaneStateSpace.hpp

@@ -3,6 +3,12 @@
 #include <Eigen/Dense>
 #include <rrt/StateSpace.hpp>
 
+#include <cmath>
+#include <algorithm>
+#include <iostream>
+
+using namespace std;
+
 namespace RRT {
 
 /**
@@ -18,6 +24,39 @@ class PlaneStateSpace : public StateSpace<POINT_CLASS> {
         return POINT_CLASS(drand48() * width(), drand48() * height());
     }
 
+    /**
+     * Generates a point based on a goal bias and a goal state
+     *
+     * @params goalState - Point that we want to extend towards
+     * @params goalBias - Changes likelihood of choosing a point near goal state
+     * @returns POINT_CLASS - random point based on goal bias and goal state
+     */
+    POINT_CLASS randomBiasState(const POINT_CLASS& goalState, float goalBias) const {
+        // Generates random value based on goalBias
+        float logitX = logit(goalBias, drand48() * .8 + .1);
+        float logitY = logit(goalBias, drand48() * .8 + .1);
+        float offsetY = 0;
+        float offsetX = 0;
+
+        // Scale X value based on distance from border
+        if (logitX > 0) {
+           offsetX = (width() - goalState.x()) / log(9) * logitX;
+        } else if (logitX < 0) {
+            offsetX = goalState.x() / log(9) * logitX;
+        }
+
+        // Scale Y value based on distance from border
+        if (logitY > 0) {
+            offsetY = (height() - goalState.y()) / log(9) * logitY;
+        } else if (logitY < 0) {
+            offsetY = goalState.y() / log(9) * logitY;
+        }
+
+        int randX = goalState.x() + offsetX, width();
+        int randY = goalState.y() + offsetY, height();
+        return POINT_CLASS(randX, randY);
+    }
+
     POINT_CLASS intermediateState(const POINT_CLASS& source,
                                   const POINT_CLASS& target,
                                   float stepSize) const {
@@ -41,6 +80,17 @@ class PlaneStateSpace : public StateSpace<POINT_CLASS> {
                pt.y() < height();
     }
 
+    /**
+     * Uses the logit function to generate a random value based on a goal bias
+     *
+     * @params goalBias - increases / decreases distance from 0
+     * @params num - value to be inputted into logit function
+     * @returns float - output of logit function scaled based on goal bias
+     */
+    float logit(const float goalBias, const float num) const{
+        return (1 - goalBias) * log(num/(1 - num));
+    }
+
     float width() const { return _width; }
     float height() const { return _height; }
 

src/rrt/StateSpace.hpp

@@ -22,6 +22,14 @@ class StateSpace {
      */
     virtual T randomState() const = 0;
 
+    /**
+     * Generates a point within the bounds of the state space
+     * based on the goalBias
+     *
+     * @return A biased state
+     */
+    virtual T randomBiasState(const T& goalState, float goalBias) const = 0;
+
     /**
      * Finds a state in the direction of @target from @source.state().
      * This new state will potentially be added to the tree.  No need to do

src/rrt/Tree.hpp

@@ -235,19 +235,17 @@ class Tree {
      * This is called at each iteration of the run() method.
      */
     Node<T>* grow() {
-        //  extend towards goal, waypoint, or random state depending on the
-        //  biases
-        //  and a random number
+         // extend towards goal, waypoint, or random state depending on the
+         // biases
+         // and a random number
         float r =
             rand() /
             (float)RAND_MAX;  //  r is between 0 and one since we normalize it
-        if (r < goalBias()) {
-            return extend(goalState());
-        } else if (r < goalBias() + waypointBias() && _waypoints.size() > 0) {
+        if (r < goalBias() + waypointBias() && _waypoints.size() > 0) {
             const T& waypoint = _waypoints[rand() % _waypoints.size()];
             return extend(waypoint);
         } else {
-            return extend(_stateSpace->randomState());
+            return extend(_stateSpace->randomBiasState(goalState(), goalBias()));
         }
     }
 
@@ -339,7 +337,6 @@ class Tree {
                 nodes.push_back(node);
                 node = node->parent();
             }
-
             // pass them one-by-one to the callback, reversing the order so
             // that the callback is called with the start point first and the
             // dest point last