SpatialTree extension problem/bug

[kb0n]

Hi,

I encountered some planning problems for a robot with circular joints using BiRRT (BaseManipulation module with MoveActiveJoints, after IK succeeded). At first I thought it was because of the circular joitns (see 1), but I figured out that it only occured with configs that are "further away" (lets say 70cm straight line in workspace). I can get to those configurations when I plan to an intermediate position first though.

I finally tracked it down to be related to the SpatialTree, more precisely its Extend(...) and _InsertNode(...) methods. This is the part in rplanners.h:

// Extend(...)
[319]  for(int iter = 0; iter < 100; ++iter) {     // to avoid infinite loops
[...]    ...
[370]    NodePtr pnewnode = _InsertNode(pnode, _vNewConfig, 0); ///< set userdata to 0
[371]    if( !!pnewnode ) {
[...]      ...
[378]    }
[...]    ...

// _InsertNode(...)
[645]  if( _numnodes == 0 ) {
[...]    ...
[651]  else {
[...]    ...
[655]    int nParentFound = _InsertRecursive(newnode, _vCurrentLevelNodes, _maxlevel, _fMaxLevelBound);
[656]    BOOST_ASSERT(nParentFound!=0);
[...]    ...

As far as I can tell, at some point in PlanPath(...) (rrt.h) the trees are extended if no path has been found yet. At some point though, some node cannot be iserted anymore by _InsertRecursive(...), which causes a chain of failures: BOOST_ASSERT(...) fails => _InsertNode(...) fails => Extend(...) fails => PlanPath(...) fails.

I'm sure you had something in mind when coding these, so I think you probably know best how to fix it. I for myself replaced the BOOST_ASSERT(...) by

[656]    if( nParentFound==0 ) {
[657]      return NULL;
[658]    } else if( nParentFound < 0 ) {
[...]      ...

and added a BOOST_ASSERT(...) whenever InsertNode(..) is called in rrt.h. I haven't tested it thoroughly yet, but I tested it a few times and I didn't have those planning errors (1) anymore :)

Cheers

[rdiankov]

thanks for the report! yup, this should be fixed asap, i'll take a look at it in the following week.

[kb0n]

It's been a while. What's the progress on this?

[rdiankov]

sorry for taking so long to respond. I've been testing this planner non-stop for the past 2 years for thousands of hours on 20+ different robots and it hasn't popped up yet, so I think it is because of circular joints. Most likely how we compute _maxdistance in SpatialTree is wrong, we set it as:

params->_distmetricfn(params->_vConfigLowerLimit, params->_vConfigUpperLimit)

but for circular joints, we might get something that is farther. The correct fix would be to allow a max number of cycles for the circular joints (like 2 or 3?) when computing the distance metric.