/* ********************************************************************************************* */
bool RRT::connect(const VectorXd &target) {
// Get the index of the nearest neighbor in the tree to the given target
int NNidx = getNearestNeighbor(target);
// Keep taking steps towards the target until a collision happens
StepResult result = STEP_PROGRESS;
while(result == STEP_PROGRESS) {
result = tryStepFromNode(target, NNidx);
NNidx = configVector.size() - 1;
}
return (result == STEP_REACHED);
}
/**
* @function connect
* @brief Connect the closest node with _target, stop until it is reached or until it collides
*/
bool B1RRT::connect( const Eigen::VectorXd &_target ) {
int NNIdx = getNearestNeighbor( _target );
StepResult result = STEP_PROGRESS;
int i = 0;
while( result == STEP_PROGRESS ) {
result = tryStepFromNode( _target, NNIdx );
NNIdx = configVector.size() -1;
i++;
}
return ( result == STEP_REACHED );
}
/**
* @function connectHeuristic
* @brief Connect the closest node with _target, stop until it is reached or until it collides
*/
bool B1RRT::connectHeuristic( int _nearId, const Eigen::VectorXd &_target ) {
int nearId = _nearId;
int oldId;
StepResult result = STEP_PROGRESS;
int i = 0;
do {
result = tryStepFromNode( _target, nearId );
oldId = nearId;
nearId = configVector.size() -1;
i++;
} while( result == STEP_PROGRESS &&
GoalDist(nearId, targetPose) < GoalDist(oldId,targetPose) );
if( result == STEP_COLLISION ) {
popRanking();
}
return ( result == STEP_REACHED );
}
/* ********************************************************************************************* */
RRT::StepResult RRT::tryStep(const VectorXd &qtry) {
int NNidx = getNearestNeighbor(qtry);
return tryStepFromNode(qtry, NNidx);
}
/**
* @function tryStep
* @brief Try to advance one stepSize towards _qtry
*/
B1RRT::StepResult B1RRT::tryStep( const Eigen::VectorXd &_qtry ) {
int NNIdx = getNearestNeighbor( _qtry );
return tryStepFromNode( _qtry, NNIdx );
}
