From f848db6763f1d2c790be2876e1f088b0b931e3b0 Mon Sep 17 00:00:00 2001 From: Jason Chan Date: Sun, 26 Feb 2017 19:35:08 +0000 Subject: [PATCH 1/6] make randomBiasState but doesnt work --- src/rrt/2dplane/PlaneStateSpace.hpp | 19 +++++++++++++++++++ src/rrt/StateSpace.hpp | 8 ++++++++ src/rrt/Tree.hpp | 24 ++++++++++++------------ 3 files changed, 39 insertions(+), 12 deletions(-) diff --git a/src/rrt/2dplane/PlaneStateSpace.hpp b/src/rrt/2dplane/PlaneStateSpace.hpp index 27c07aa..599e1e9 100644 --- a/src/rrt/2dplane/PlaneStateSpace.hpp +++ b/src/rrt/2dplane/PlaneStateSpace.hpp @@ -3,6 +3,12 @@ #include #include +#include +#include +#include + +using namespace std; + namespace RRT { /** @@ -18,6 +24,15 @@ class PlaneStateSpace : public StateSpace { return POINT_CLASS(drand48() * width(), drand48() * height()); } + // POINT_CLASS randomBiasState(const POINT_CLASS& goalState, float goalBias) const { + POINT_CLASS randomBiasState(const POINT_CLASS& goalState, float goalBias) const { + // ensures that randX and randY are within the fields bounds + int randX = std::max(std::min(goalState.x() + logit(goalBias, drand48()), width() / 2), width() / 2 * -1); + cout << width() << endl; + int randY = std::max(std::min(goalState.y() + logit(goalBias, drand48()), height() / 2), height() / 2 * -1); + return POINT_CLASS(randX, randY); + } + POINT_CLASS intermediateState(const POINT_CLASS& source, const POINT_CLASS& target, float stepSize) const { @@ -41,6 +56,10 @@ class PlaneStateSpace : public StateSpace { pt.y() < height(); } + float logit(const float goalBias, const float num) const{ + return goalBias * 2000 * log(num/(1 - num)); + } + float width() const { return _width; } float height() const { return _height; } diff --git a/src/rrt/StateSpace.hpp b/src/rrt/StateSpace.hpp index 1c627d5..e78c5fe 100644 --- a/src/rrt/StateSpace.hpp +++ b/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 diff --git a/src/rrt/Tree.hpp b/src/rrt/Tree.hpp index f807d42..cf66195 100644 --- a/src/rrt/Tree.hpp +++ b/src/rrt/Tree.hpp @@ -238,17 +238,18 @@ class Tree { // 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) { - const T& waypoint = _waypoints[rand() % _waypoints.size()]; - return extend(waypoint); - } else { - return extend(_stateSpace->randomState()); - } + // 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) { + // const T& waypoint = _waypoints[rand() % _waypoints.size()]; + // return extend(waypoint); + // } else { + // return extend(_stateSpace->randomState()); + // + return extend(_stateSpace->randomBiasState(goalState(), goalBias())); } /** @@ -339,7 +340,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 From e2da9cf0e850746a32aab00edb85916cb2b1055a Mon Sep 17 00:00:00 2001 From: Jason Chan Date: Sun, 26 Feb 2017 20:53:45 +0000 Subject: [PATCH 2/6] passable biased tree doesnt work well below .5 goal bias --- src/rrt/2dplane/PlaneStateSpace.hpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/rrt/2dplane/PlaneStateSpace.hpp b/src/rrt/2dplane/PlaneStateSpace.hpp index 599e1e9..226dac0 100644 --- a/src/rrt/2dplane/PlaneStateSpace.hpp +++ b/src/rrt/2dplane/PlaneStateSpace.hpp @@ -27,9 +27,12 @@ class PlaneStateSpace : public StateSpace { // POINT_CLASS randomBiasState(const POINT_CLASS& goalState, float goalBias) const { POINT_CLASS randomBiasState(const POINT_CLASS& goalState, float goalBias) const { // ensures that randX and randY are within the fields bounds - int randX = std::max(std::min(goalState.x() + logit(goalBias, drand48()), width() / 2), width() / 2 * -1); - cout << width() << endl; - int randY = std::max(std::min(goalState.y() + logit(goalBias, drand48()), height() / 2), height() / 2 * -1); + float offsetX = width() * logit(goalBias, drand48() * .8 + .1) + int randX = std::max(std::min(goalState.x() + offsetX, width()), 0.f); + //cout << width() << endl; + float offsetY = height() * logit(goalBias, drand48() * .8 + .1); + int randY = std::max(std::min(goalState.y() + offsetY, height()), 0.f); + cout << "Point: " << "(" << randX << ", " << randY << ")" << endl; return POINT_CLASS(randX, randY); } @@ -57,7 +60,9 @@ class PlaneStateSpace : public StateSpace { } float logit(const float goalBias, const float num) const{ - return goalBias * 2000 * log(num/(1 - num)); + float x = (1 - goalBias) * log(num/(1 - num)); + cout << "logit " << x << endl; + return x; } float width() const { return _width; } From d17729f39f165f32d488047769102d09d34ff8af Mon Sep 17 00:00:00 2001 From: Jason Chan Date: Wed, 1 Mar 2017 01:20:58 +0000 Subject: [PATCH 3/6] x and y are scaled by distance from border --- src/rrt/2dplane/PlaneStateSpace.hpp | 48 ++++++++++++++++++++++------- src/rrt/Tree.hpp | 27 ++++++++-------- 2 files changed, 49 insertions(+), 26 deletions(-) diff --git a/src/rrt/2dplane/PlaneStateSpace.hpp b/src/rrt/2dplane/PlaneStateSpace.hpp index 226dac0..5f6b7bc 100644 --- a/src/rrt/2dplane/PlaneStateSpace.hpp +++ b/src/rrt/2dplane/PlaneStateSpace.hpp @@ -24,15 +24,36 @@ class PlaneStateSpace : public StateSpace { return POINT_CLASS(drand48() * width(), drand48() * height()); } - // POINT_CLASS randomBiasState(const POINT_CLASS& goalState, float goalBias) const { + /** + * 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 { - // ensures that randX and randY are within the fields bounds - float offsetX = width() * logit(goalBias, drand48() * .8 + .1) - int randX = std::max(std::min(goalState.x() + offsetX, width()), 0.f); - //cout << width() << endl; - float offsetY = height() * logit(goalBias, drand48() * .8 + .1); - int randY = std::max(std::min(goalState.y() + offsetY, height()), 0.f); - cout << "Point: " << "(" << randX << ", " << randY << ")" << endl; + // 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); } @@ -59,10 +80,15 @@ class PlaneStateSpace : public StateSpace { 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{ - float x = (1 - goalBias) * log(num/(1 - num)); - cout << "logit " << x << endl; - return x; + return (1 - goalBias) * log(num/(1 - num)); } float width() const { return _width; } diff --git a/src/rrt/Tree.hpp b/src/rrt/Tree.hpp index cf66195..5e974b7 100644 --- a/src/rrt/Tree.hpp +++ b/src/rrt/Tree.hpp @@ -235,21 +235,18 @@ class Tree { * This is called at each iteration of the run() method. */ Node* grow() { - // 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) { - // const T& waypoint = _waypoints[rand() % _waypoints.size()]; - // return extend(waypoint); - // } else { - // return extend(_stateSpace->randomState()); - // - return extend(_stateSpace->randomBiasState(goalState(), goalBias())); + // 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() + waypointBias() && _waypoints.size() > 0) { + const T& waypoint = _waypoints[rand() % _waypoints.size()]; + return extend(waypoint); + } else { + return extend(_stateSpace->randomBiasState(goalState(), goalBias())); + } } /** From 1596f0086e4d41fb346a51bc377c7df4936c519c Mon Sep 17 00:00:00 2001 From: Jason Chan Date: Sun, 5 Mar 2017 18:58:29 +0000 Subject: [PATCH 4/6] chance bounds to .95 instead of .9 --- src/rrt/2dplane/PlaneStateSpace.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/rrt/2dplane/PlaneStateSpace.hpp b/src/rrt/2dplane/PlaneStateSpace.hpp index 5f6b7bc..6397ea2 100644 --- a/src/rrt/2dplane/PlaneStateSpace.hpp +++ b/src/rrt/2dplane/PlaneStateSpace.hpp @@ -33,23 +33,23 @@ class PlaneStateSpace : public StateSpace { */ 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 logitX = logit(goalBias, drand48() * .9 + .05); + float logitY = logit(goalBias, drand48() * .9 + .05); float offsetY = 0; float offsetX = 0; // Scale X value based on distance from border if (logitX > 0) { - offsetX = (width() - goalState.x()) / log(9) * logitX; + offsetX = (width() - goalState.x()) / log(19) * logitX; } else if (logitX < 0) { - offsetX = goalState.x() / log(9) * logitX; + offsetX = goalState.x() / log(19) * logitX; } // Scale Y value based on distance from border if (logitY > 0) { - offsetY = (height() - goalState.y()) / log(9) * logitY; + offsetY = (height() - goalState.y()) / log(19) * logitY; } else if (logitY < 0) { - offsetY = goalState.y() / log(9) * logitY; + offsetY = goalState.y() / log(19) * logitY; } int randX = goalState.x() + offsetX, width(); From 371876c13c19383cd916459592fb0c538cc13104 Mon Sep 17 00:00:00 2001 From: Jason Chan Date: Sun, 5 Mar 2017 20:53:24 +0000 Subject: [PATCH 5/6] remove log bounds, seems to remove rectangular tree --- src/rrt/2dplane/PlaneStateSpace.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/rrt/2dplane/PlaneStateSpace.hpp b/src/rrt/2dplane/PlaneStateSpace.hpp index 6397ea2..5f5463a 100644 --- a/src/rrt/2dplane/PlaneStateSpace.hpp +++ b/src/rrt/2dplane/PlaneStateSpace.hpp @@ -33,27 +33,27 @@ class PlaneStateSpace : public StateSpace { */ POINT_CLASS randomBiasState(const POINT_CLASS& goalState, float goalBias) const { // Generates random value based on goalBias - float logitX = logit(goalBias, drand48() * .9 + .05); - float logitY = logit(goalBias, drand48() * .9 + .05); + float logitX = logit(goalBias, (float) rand() / RAND_MAX); + float logitY = logit(goalBias, (float) rand() / RAND_MAX); float offsetY = 0; float offsetX = 0; // Scale X value based on distance from border if (logitX > 0) { - offsetX = (width() - goalState.x()) / log(19) * logitX; + offsetX = (width() - goalState.x()) * logitX; } else if (logitX < 0) { - offsetX = goalState.x() / log(19) * logitX; + offsetX = goalState.x() * logitX; } // Scale Y value based on distance from border if (logitY > 0) { - offsetY = (height() - goalState.y()) / log(19) * logitY; + offsetY = (height() - goalState.y()) * logitY; } else if (logitY < 0) { - offsetY = goalState.y() / log(19) * logitY; + offsetY = goalState.y() * logitY; } - int randX = goalState.x() + offsetX, width(); - int randY = goalState.y() + offsetY, height(); + int randX = goalState.x() + offsetX; + int randY = goalState.y() + offsetY; return POINT_CLASS(randX, randY); } From f3e0df8589a6988b2b00d9d0c9986eef7177bd44 Mon Sep 17 00:00:00 2001 From: Jason Chan Date: Tue, 4 Apr 2017 23:25:22 +0000 Subject: [PATCH 6/6] finish randomBiasState --- src/rrt/2dplane/PlaneStateSpace.hpp | 18 +++++------------- src/rrt/Tree.hpp | 6 ++++-- 2 files changed, 9 insertions(+), 15 deletions(-) diff --git a/src/rrt/2dplane/PlaneStateSpace.hpp b/src/rrt/2dplane/PlaneStateSpace.hpp index 5f5463a..13aebc8 100644 --- a/src/rrt/2dplane/PlaneStateSpace.hpp +++ b/src/rrt/2dplane/PlaneStateSpace.hpp @@ -33,25 +33,17 @@ class PlaneStateSpace : public StateSpace { */ POINT_CLASS randomBiasState(const POINT_CLASS& goalState, float goalBias) const { // Generates random value based on goalBias - float logitX = logit(goalBias, (float) rand() / RAND_MAX); - float logitY = logit(goalBias, (float) rand() / RAND_MAX); + float logitX = logit(goalBias / 3 + .67, (float) rand() / RAND_MAX); + float logitY = logit(goalBias / 3 + .67, (float) rand() / RAND_MAX); float offsetY = 0; float offsetX = 0; // Scale X value based on distance from border - if (logitX > 0) { - offsetX = (width() - goalState.x()) * logitX; - } else if (logitX < 0) { - offsetX = goalState.x() * logitX; - } + offsetX = std::max(goalState.x(), width() - goalState.x()) * logitX; // Scale Y value based on distance from border - if (logitY > 0) { - offsetY = (height() - goalState.y()) * logitY; - } else if (logitY < 0) { - offsetY = goalState.y() * logitY; - } - + offsetY = std::max(goalState.y(), height() - goalState.y()) * logitY; + int randX = goalState.x() + offsetX; int randY = goalState.y() + offsetY; return POINT_CLASS(randX, randY); diff --git a/src/rrt/Tree.hpp b/src/rrt/Tree.hpp index 5e974b7..321e7a1 100644 --- a/src/rrt/Tree.hpp +++ b/src/rrt/Tree.hpp @@ -241,11 +241,13 @@ class Tree { float r = rand() / (float)RAND_MAX; // r is between 0 and one since we normalize it - if (r < goalBias() + waypointBias() && _waypoints.size() > 0) { + if (r < goalBias()) { + return extend(goalState()); + } else if (r < goalBias() + waypointBias() && _waypoints.size() > 0) { const T& waypoint = _waypoints[rand() % _waypoints.size()]; return extend(waypoint); } else { - return extend(_stateSpace->randomBiasState(goalState(), goalBias())); + return extend(_stateSpace->randomState()); } }