DATA-776 Motion planning redesign for speed and quality #1604
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
viambot
added
the
safe to test
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
raybjork
requested changes
Nov 23, 2022
There was a problem hiding this comment.
Ok I think this is the last round of changes I'm going to request, its looking like its in a really great spot now! These abstractions make the whole thing a lot cleaner, I'm in particular really liking how easily you were able to add the fallback options, as well as the Planager
motionplan/rrtStarConnect.go
Outdated
|
|
||
| solution := shortestPath(rrt.rm, shared, optimalCost) | ||
| solutionCost = EvaluatePlan(solution, rrt.planOpts) | ||
| mp.logger.Warnf("RRT* progress: %d%%\tpath cost: %.3f", 100*i/algOpts.PlanIter, solutionCost) | ||
| // check if an early exit is possible | ||
| if solutionCost-optimalCost < algOpts.OptimalityThreshold*optimalCost { |
There was a problem hiding this comment.
[minor] I wonder how often this is happening. Now that we are directly checking for interpolations, my hunch is this will never evaluate to true. We could save ourselves an if statement per iteration by removing this. As well as clean up the code a little more
There was a problem hiding this comment.
and if we do that there is also no point of even having an OptimalityThreshold
There was a problem hiding this comment.
Good point, I will remove.
| @@ -209,12 +233,14 @@ func (mp *rrtStarConnectMotionPlanner) extend(algOpts *rrtStarConnectOptions, tr | |||
| if cost < minCost && mp.checkPath(algOpts.planOpts, neighborNode.Q(), target) { | |||
| minIndex = i | |||
| minCost = cost | |||
| break | |||
There was a problem hiding this comment.
can you explain why we're breaking out now?
There was a problem hiding this comment.
kNearestNeighbors was updated to return the neighbors sorted by cost (if they are cost nodes).
So the first valid node we find, is going to be the minimum cost.
There was a problem hiding this comment.
sweet. Can you add a comment explaining this?
motionplan/planManager.go
Outdated
| seed []referenceframe.Input, | ||
| opt *plannerOptions, | ||
| ) ([][]referenceframe.Input, error) { | ||
| return nil, nil |
There was a problem hiding this comment.
This is TBD i take it? Maybe should remove this until we're ready to write it so its not sitting here returning nils
There was a problem hiding this comment.
This is unused and unneeded; surprised no one noticed before now. Deleting.
biotinker
force-pushed
the
20221101_motion_planning_speedup
branch
from
a99bd55 to
9a58069
Compare
viambot
added
safe to test
viambot
added
safe to test
|
raybjork
approved these changes
Nov 23, 2022
| @@ -209,12 +233,14 @@ func (mp *rrtStarConnectMotionPlanner) extend(algOpts *rrtStarConnectOptions, tr | |||
| if cost < minCost && mp.checkPath(algOpts.planOpts, neighborNode.Q(), target) { | |||
| minIndex = i | |||
| minCost = cost | |||
| break | |||
There was a problem hiding this comment.
sweet. Can you add a comment explaining this?
raybjork
changed the title
mcvella
approved these changes
Nov 23, 2022
Closed
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Firstly, this comes with a significant speed boost, solving most motion path problems in under 1/3 the time.
Secondly, this simplifies the amount of motion planning that is surfaced to the rest of RDK. All structs, algorithms, etc are now private. Instead, helper functions in
motionPlanner.goare now the Official, and indeed only, way to get motion steps for your component. Arms, etc have been updated to use these functions.