/**
* @brief Check if any of the waypoints has nan coordinates
*
* @param road ...
* @return bool
*/
bool ElasticBand::checkIfNAN(const WayPoints &road)
{
for (auto it = road.begin(); it != road.end(); ++it)
if (std::isnan(it->pos.x()) or std::isnan(it->pos.y()) or std::isnan(it->pos.z()))
{
road.print();
return true;
}
return false;
}
float ElasticBand::computeForces(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
if (road.size() < 3)
return 0;
// To avoid moving the rotation element attached to the last
int lastP;
if (road.last().hasRotation)
lastP = road.size() - 2;
else
lastP = road.size() - 1;
// Go through all points in the road
float totalChange = 0.f;
for (int i = 1; i < lastP; i++)
{
if (road[i].isVisible == false)
break;
WayPoint &w0 = road[i - 1];
WayPoint &w1 = road[i];
WayPoint &w2 = road[i + 1];
// Atraction force caused by the trajectory stiffnes, trying to straighten itself. It is computed as a measure of local curvature
QVec atractionForce(3);
float n = (w0.pos - w1.pos).norm2() / ((w0.pos - w1.pos).norm2() + w1.initialDistanceToNext);
atractionForce = (w2.pos - w0.pos) * n - (w1.pos - w0.pos);
//Compute derivative of force field and store values in w1.bMinuxX .... and w1.minDist. Also variations wrt former epochs
computeDistanceField(innermodel, w1, laserData, FORCE_DISTANCE_LIMIT);
QVec repulsionForce = QVec::zeros(3);
QVec jacobian(3);
// space interval to compute the derivative. Related to to robot's size
float h = DELTA_H;
if ((w1.minDistHasChanged == true) /*and (w1.minDist < 250)*/ )
{
jacobian = QVec::vec3(w1.bMinusX - w1.bPlusX,
0,
w1.bMinusY - w1.bPlusY) * (T) (1.f / (2.f * h));
// repulsion force is computed in the direction of maximun laser-point distance variation and scaled so it is 0 is beyond FORCE_DISTANCE_LIMIT and FORCE_DISTANCE_LIMIT if w1.minDist.
repulsionForce = jacobian * (FORCE_DISTANCE_LIMIT - w1.minDist);
}
float alpha = -0.5; //Negative values between -0.1 and -1. The bigger in magnitude, the stiffer the road becomes
float beta = 0.55; //Posibite values between 0.1 and 1 The bigger in magnitude, more separation from obstacles
QVec change = (atractionForce * alpha) + (repulsionForce * beta);
if (std::isnan(change.x()) or std::isnan(change.y()) or std::isnan(change.z()))
{
road.print();
qDebug() << atractionForce << repulsionForce;
qFatal("change");
}
//Now we remove the tangencial component of the force to avoid recirculation of band points
//QVec pp = road.getTangentToCurrentPoint().getPerpendicularVector();
//QVec nChange = pp * (pp * change);
w1.pos = w1.pos - change;
totalChange = totalChange + change.norm2();
}
return totalChange;
}