Towards fully stable cost-shared RRTForest