Some tweaks to cost-shared RRTForest