/**
* @function connectGoal
*/
bool JG_RRT::connectGoal()
{
Eigen::MatrixXd J(3,7);
Eigen::MatrixXd Jinv(7,3);
Eigen::MatrixXd delta_x;
Eigen::MatrixXd delta_q;
Eigen::MatrixXd q_new;
JG_RRT::StepResult result;
double dist_new;
double dist_old;
result = STEP_PROGRESS;
dist_new = 0;
dist_old = DBL_MAX;
int i = 0;
while( result == STEP_PROGRESS && (dist_new < dist_old) )
{ //-- Get the closest node to the goal ( workspace metric )
int NNidx = pop_Ranking();
if( NNidx == -1 )
{
break;
}
Eigen::VectorXd q_closest = configVector[NNidx];
dist_old = wsDistance( q_closest );
//-- Get the Jacobian
robinaLeftArm_j( q_closest, TWBase, Tee, J );
//-- Get the pseudo-inverse (easy way)
pseudoInv( 3, 7, J, Jinv );
//-- Get the workspace
delta_x = wsDiff( q_closest );
delta_q = Jinv*delta_x;
//-- Scale -- with this 2 actually result will always be in PROGRESS, if not COLLISION
double scal = 2*stepSize/delta_q.norm();
delta_q = delta_q*scal;
//-- Get q_new
q_new = q_closest + delta_q;
//-- Attempt a new step towards J
result = tryStep( q_new, NNidx );
i++;
//-- Check
if( result == STEP_PROGRESS )
{ dist_new = wsDistance( configVector[configVector.size() - 1] );
if( dist_new < distanceThresh )
{
result = STEP_REACHED;
break;
}
}
}
return( result == STEP_REACHED );
}
/**
* @function plan
* @brief
*/
bool JG_RRT::plan( const Eigen::VectorXd &_startConfig,
const Eigen::Transform<double, 3, Eigen::Affine> &_goalPose,
const std::vector< Eigen::VectorXd > &_guidingNodes,
std::vector<Eigen::VectorXd> &path )
{
/** Store information */
this->startConfig = _startConfig;
this->goalPose = _goalPose;
this->goalPosition = _goalPose.translation();
//-- Initialize the search tree
addNode( startConfig, -1 );
//-- Add the guiding nodes
addGuidingNodes( _guidingNodes );
double p;
while( goalDistVector[activeNode] > distanceThresh )
{
//-- Generate the probability
p = RANDNM( 0.0, 1.0 );
//-- Apply either extension to goal (J) or random connection
if( p < p_goal )
{
if( extendGoal() == true )
{
break;
}
}
else
{
tryStep(); /*extendRandom();*/
}
// Check if we are still inside
if( configVector.size() > maxNodes )
{ cout<<"-- Exceeded "<<maxNodes<<" allowed - ws_dist: "<<goalDistVector[rankingVector[0]]<<endl;
break;
}
}
//-- If a path is found
if( goalDistVector[activeNode] < distanceThresh )
{ tracePath( activeNode, path );
cout<<"JG - Got a path! - nodes: "<<path.size()<<" tree size: "<<configVector.size()<<endl;
return true;
}
else
{ cout<<"--(!) JG :No successful path found! "<<endl;
return false;
}
}
/**
* @function connect
* @brief
*/
bool JG_RRT::connect( const Eigen::VectorXd &target )
{
int NNidx = getNearestNeighbor(target);
StepResult result = STEP_PROGRESS;
int i = 0;
while(result == STEP_PROGRESS)
{
result = tryStep(target, NNidx);
NNidx = configVector.size() - 1;
i++;
}
return (result == STEP_REACHED);
}
void PropagationCK::process(Candidate *candidate) const {
// save the new previous particle state
ParticleState ¤t = candidate->current;
candidate->previous = current;
double step = clip(candidate->getNextStep(), minStep, maxStep);
// rectilinear propagation for neutral particles
if (current.getCharge() == 0) {
Vector3d pos = current.getPosition();
Vector3d dir = current.getDirection();
current.setPosition(pos + dir * step);
candidate->setCurrentStep(step);
candidate->setNextStep(maxStep);
return;
}
Y yIn(current.getPosition(), current.getDirection());
Y yOut, yErr;
double h = step / c_light;
double hTry, r;
double z = candidate->getRedshift();
// try performing a step until the relative error is less than the desired
// tolerance or the minimum step size has been reached
do {
hTry = h;
tryStep(yIn, yOut, yErr, hTry, current, z);
// determine absolute direction error relative to tolerance
r = yErr.u.getR() / tolerance;
// new step size to keep the error close to the tolerance
h *= 0.95 * pow(r, -0.2);
// limit change of new step size
h = clip(h, 0.1 * hTry, 5 * hTry);
} while (r > 1 && h > minStep / c_light);
current.setPosition(yOut.x);
current.setDirection(yOut.u.getUnitVector());
candidate->setCurrentStep(hTry * c_light);
candidate->setNextStep(h * c_light);
}
/**
* @function tryStep
*/
JG_RRT::StepResult JG_RRT::tryStep( const Eigen::VectorXd &qtry )
{
int NNidx = getNearestNeighbor( qtry );
return tryStep( qtry, NNidx );
}
/**
* @function tryStep
*/
JG_RRT::StepResult JG_RRT::tryStep()
{
Eigen::VectorXd qtry = getRandomConfig();
return tryStep(qtry);
}
/* ********************************************************************************************* */
RRT::StepResult RRT::tryStep() {
VectorXd qtry = getRandomConfig();
return tryStep(qtry);
}
void rayCast(Vector2i src, Vector2i dst, RAY_CALLBACK callback, void *data)
{
if (!callback(src, 0, data) || src == dst) // Start at src.
{
return; // Callback gave up after the first point, or there are no other points.
}
Vector2i srcM = map_coord(src);
Vector2i dstM = map_coord(dst);
Vector2i step, tile, cur, end;
initSteps(srcM.x, dstM.x, tile.x, step.x, cur.x, end.x);
initSteps(srcM.y, dstM.y, tile.y, step.y, cur.y, end.y);
Vector2i prev(0, 0); // Dummy initialisation.
bool first = true;
Vector2i nextX(0, 0), nextY(0, 0); // Dummy initialisations.
bool canX = tryStep(tile.x, step.x, cur.x, end.x, nextX.x, nextX.y, src.x, src.y, dst.x, dst.y);
bool canY = tryStep(tile.y, step.y, cur.y, end.y, nextY.y, nextY.x, src.y, src.x, dst.y, dst.x);
while (canX || canY)
{
int32_t xDist = abs(nextX.x - src.x) + abs(nextX.y - src.y);
int32_t yDist = abs(nextY.x - src.x) + abs(nextY.y - src.y);
Vector2i sel;
Vector2i selTile;
if (canX && (!canY || xDist < yDist)) // The line crosses a vertical grid line next.
{
sel = nextX;
selTile = tile;
canX = tryStep(tile.x, step.x, cur.x, end.x, nextX.x, nextX.y, src.x, src.y, dst.x, dst.y);
}
else // The line crosses a horizontal grid line next.
{
assert(canY);
sel = nextY;
selTile = tile;
canY = tryStep(tile.y, step.y, cur.y, end.y, nextY.y, nextY.x, src.y, src.x, dst.y, dst.x);
}
if (!first)
{
// Find midpoint.
Vector2i avg = (prev + sel) / 2;
// But make sure it's on the right tile, since it could be off-by-one if the line passes exactly through a grid intersection.
avg.x = std::min(std::max(avg.x, world_coord(selTile.x)), world_coord(selTile.x + 1) - 1);
avg.y = std::min(std::max(avg.y, world_coord(selTile.y)), world_coord(selTile.y + 1) - 1);
if (!worldOnMap(avg) || !callback(avg, iHypot(avg), data))
{
return; // Callback doesn't want any more points, or we reached the edge of the map, so return.
}
}
prev = sel;
first = false;
}
// Include the endpoint.
if (!worldOnMap(dst))
{
return; // Stop, since reached the edge of the map.
}
callback(dst, iHypot(dst), data);
}
