Implemented adaptive stepsize control

[joshhting]

The rrt application now has a checkbox that, if checked, runs the algorithm with adaptive stepsize control. The path takes smaller, more careful steps when there are obstacles nearby, and takes larger steps otherwise in order to save time safely.

[justbuchanan]

Sorry for taking so long to get back to this, it sorta slipped my mind. In the future, just shoot me a message or comment on here when you've updated things - I unfortunately don't get notifications for new commits being pushed.

Anyways, I commented on a few things, let me know if you have any questions.

[joshhting]

Just committed edits to everything you pointed out.

[justbuchanan]

Cool, it's looking good! A few more minor things:

• In the StateSpace class, you have two versions of the intermediateState() function - one for fixed step size and one for ASC. I think since we've got the two versions, we can do away with having negative step size values indicating ASC in the first version. Then the overloaded version will be solely responsible for ASC.
• You changed the isDynamic() in BiTree to isASC(), but the corresponding method in Tree still has the old name. Also, I think it'd be a little clearer if we went with isAscEnabled() and setAscEnabled(). Let's rename the instance variable as well to match.
• I tried running it with the ASC checkbox checked and I couldn't tell if it was working - it seemed to use the same step size everywhere regardless of where how close it was to obstacles. Here's a screenshot:

[justbuchanan]

@joshhting, when you get a chance, can you put a short description of asc at the top of the Tree.hpp file and describe how to choose the parameters? Also, I realized there was one more thing I needed to change to get googletest working again and pushed the fix to master - if you merge master in again it should work.

[justbuchanan]

Just noticed an issue on the master branch where the _goalMaxDist property was never looked at. It's fixed as of 17bf0fc57b8cbf3b465d34f49f054a83fd9143cf and I added a new test case to make sure similar issues are caught next time. Take a look at that commit and see the changes to the test cases, you'll have to make a couple minor changes to your ASC test case.

[justbuchanan]

@joshhting, I played around with this some and put my changes/hacks on the justin/asc-mods branch. The main thing I changed was the method of calculating the step size. Yours multiplies or divides by a factor at each step depending on distance, while mine just uses a step proportional to the nearest obstacle (within preset bounds). I also fixed some issues with the nearestObstacleDist() method that were causing some problems.

Here's a run of yours:

And here's a screenshot of it with my changes:

Note that in yours there are several instances where it chooses a small step size despite there being no obstacles near the point.

[joshhting]

rrt is now perfect

[justbuchanan]

Looks great! I made a few more notes - mostly just to update comments and code to reflect the latest changes. And to remove code in a couple places that was added, but no longer needed.

[joshhting]

Is this good to merge now?

[justbuchanan]

Just about, I still think there's a small issue in ObstacleGrid::nearestObstacleDist() though. Take a look at that one again and see if my question/explanation makes sense. The way your implementation is setup, it's doing a search in a rectangle around the point, not a circle, so the range of x and y values you search through should be based on that search rectangle.

An alternative, but more complex, implementation would be to search in a circle centered around the query point. I like your current way of doing it better for simplicity, but I don't think you need the SQRT2 factor in there.

[joshhting]

What if instead of doing either of those we started at the center point and then spiraled outwards from there? So we would search in a box perimeter around the point, then a perimeter around that, etc. This way the first obstacle we encounter would more or less be the nearest obstacle and then we could just stop searching shortly after we find one.

If we do this, we would need to search a few iterations after we find one in case the point we found was near the corner of the box we were searching, because there could end up being a closer obstacle in the next largest box near the center of one of the box's edges.

[justbuchanan]

Yeah that's not a bad idea! It could certainly improve the performance of that method a good bit.

One thing to consider though is that the ObstacleGrid class is mostly intended for use with the RRT Viewer app. When we integrate this RRT stuff into soccer, our obstacles will be handled much differently. In soccer, the obstacle set will be a set of Shape objects rather than a grid.

If you'd like to implement the spiral way of doing this, go for it, but keep in mind that it won't improve the real-life usage of our RRTs, only the performance of the demo app. It may be better to just go for the simple solution here even if it isn't the fastest.

[justbuchanan]

I see that you've already updated that method already and fixed the previous issue. I looked through it all again and I think it's good to go! Go ahead and merge it if it's ready and tag a new release. This is under "releases" on the main page of the repo. We're on 0.4.0 right now, so you can make this 0.5.0. Then just put something in the release notes about the addition of adaptive step size control.