fix strrt distance function: return inf when t1 > t2, and change the order of the arguments in distance function for goal tree. #1177
Conversation
…order of the arguments in distance function for goal tree.
| tGoal_.reset(new ompl::NearestNeighborsLinear<Motion *>()); | ||
| tStart_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); }); | ||
| tGoal_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); }); | ||
| tGoal_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(b, a); }); |
There was a problem hiding this comment.
I made a mistake, in NearestNeighborsLinear argument b is for the state, for which we are finding neighbours,
I fixed it locally, but strrt* does not work, with or without this fix. This means there is another problem.
There was a problem hiding this comment.
Hi @kaizer-nurik. Did you check if you can run the demo scripts of ST-RRT* in the demo folder? It is important to define a MotionValidator as described in the SpaceTimePlanning.cpp demo file, so that no negative time edges are added. If you still have issues, please don't hesitate to contact the author @frangrothe.
|
Hi @aorthey , I found the issue. I forgot to modify distance in rewireGoalTree and growTreeSingle. Now, my application and demo script SpaceTimePlanning.cpp works fine. I didn't notice significant improvements in single tests, but theoretically, this fixes can improve performance in hard tests by speeding up nearestR (because there won't be states, that are close enough, but t1>t2) or by helping nearestK (states with t1>t2 won't take up space of valid states in array size of K). Here is explanation of my changes:
also, query state is passed as second argument in nearestR and nearestK functions. in function ompl::geometric::STRRTstar::growTree, if start tree is growing, that means, that query state in tree->nearest() is in future, while we want to find the nearest state in start tree in the past (so that t1 < t2). Thats why, query state must be passed as second argument to distance function, and thats why line is correct Similarly, in function ompl::geometric::STRRTstar::growTree, if goal tree is growing, query state in tree->nearest() must be passed in distance function as first argument, because is is in the "past", and nearest goal tree states are in the "future" (so t1<t2). Thats why line is correct; |
|
|
||
| double d = 0; | ||
| if(tgi.start){ | ||
| d = si_->distance(nmotion->state, rmotion->state); | ||
| } | ||
| else{ | ||
| d = si_->distance( rmotion->state,nmotion->state); | ||
| } |
There was a problem hiding this comment.
- in function ompl::geometric::STRRTstar::growTreeSingle, when we are growing from start tree, nmotion->state is "past", and rmotion->state is "future", so to satisfy t1<t2, we must pass this arguments as si_->distance(nmotion->state, rmotion->state);
When we are growing from goal tree, nmotion->state is "future", and rmotion->state is "past", so to satisfy t1<t2, we must pass this arguments as si_->distance( rmotion->state,nmotion->state);
| tGoal_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(a, b); }); | ||
| getNeighbors(tGoal_, addedMotion, nbh); | ||
| tGoal_->setDistanceFunction([this](const Motion *a, const Motion *b) { return distanceFunction(b, a); }); |
There was a problem hiding this comment.
- In ompl::geometric::STRRTstar::rewireGoalTree, when we are finding candidates for rewiring, we expect them to be in the "past", so our query state "addedMotion" is in the future. So, to satisfy t1<t2, we must change distance function, and pass query state as second argument in distance function. (Inside nearestK and nearestR the query state is passed as second argument "b" )
in original paper of STRRT* distance function is defined as
As we see, if t1 > t2, than distance function returns infinity, but in SpaceTimeStateSpace if t1>t2, it just returns the absolute value of |t1-t2|. This leads to incorrect work of nearest neighbours, that returns invalid states. In this pull request i add a check of (t1>t2) in SpaceTimeStateSpace, so it can be identical to the original distance function, that is defined in paper.
Also, i changed order of arguments in distanceFunction for goal tree, because it grows backwards.