/** * @brief A point of the road is visible if it is between the robot and the laser beam running through it, and if the previous point was visible * All points in the road are updated * @param road ... * @param laserData ... * @return bool */ bool ElasticBand::checkVisiblePoints(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData) { //Simplify laser polyline using Ramer-Douglas-Peucker algorithm std::vector<Point> points, res; QVec wd; for (auto &ld : laserData) { //wd = innermodel->laserTo("world", "laser", ld.dist, ld.angle); //OPTIMIZE THIS FOR ALL CLASS METHODS wd = innermodel->getNode<InnerModelLaser>("laser")->laserTo("world", ld.dist, ld.angle); points.push_back(Point(wd.x(), wd.z())); } res = simPath.simplifyWithRDP(points, 70); //qDebug() << __FUNCTION__ << "laser polygon after simp" << res.size(); // Create a QPolygon so we can check if robot outline falls inside QPolygonF polygon; for (auto &p: res) polygon << QPointF(p.x, p.y); // Move the robot along the road int robot = road.getIndexOfNextPoint(); QVec memo = innermodel->transform6D("world", "robot"); for(int it = robot; it<road.size(); ++it) { road[it].isVisible = true; innermodel->updateTransformValues("robot", road[it].pos.x(), road[it].pos.y(), road[it].pos.z(), 0, road[it].rot.y(), 0); //get Robot transformation matrix QMat m = innermodel->getTransformationMatrix("world", "robot"); // Transform all points at one to world RS //m.print("m"); //pointsMat.print("pointsMat"); QMat newPoints = m * pointsMat; //Check if they are inside the laser polygon for (int i = 0; i < newPoints.nCols(); i++) { // qDebug() << __FUNCTION__ << "----------------------------------"; // qDebug() << __FUNCTION__ << QPointF(newPoints(0, i), newPoints(2, i)); // qDebug() << __FUNCTION__ << polygon; if (polygon.containsPoint(QPointF(newPoints(0, i), newPoints(2, i)),Qt::OddEvenFill) == false) { road[it].isVisible = false; //qFatal("fary"); break; } } // if( road[it].isVisible == false) // { // for (int k = it; k < road.size(); ++k) // road[k].isVisible = false; // break; // } } // Set the robot back to its original state innermodel->updateTransformValues("robot", memo.x(), memo.y(), memo.z(), 0, memo.ry(), 0); //road.print(); return true; }
/** * @brief Adds points to the band if two existing ones are too far apart (ROBOT_RADIUS) * * @param road ... * @return void */ void ElasticBand::addPoints(WayPoints &road, const CurrentTarget ¤tTarget) { int offset = 1; for (int i = 0; i < road.size() - offset; i++) { if (i > 0 and road[i].isVisible == false) break; WayPoint &w = road[i]; WayPoint &wNext = road[i + 1]; float dist = (w.pos - wNext.pos).norm2(); if (dist > ROAD_STEP_SEPARATION) //SHOULD GET FROM IM { float l = 0.9 * ROAD_STEP_SEPARATION / dist; //Crucial que el punto se ponga mas cerca que la condición de entrada WayPoint wNew((w.pos * (1 - l)) + (wNext.pos * l)); road.insert(i + 1, wNew); } } //ELIMINATED AS REQUESTED BY MANSO //Move point before last to orient the robot. This works but only if the robots approaches from the lower quadrants //The angle formed by this point and the last one has to be the same es specified in the target //We solve this equations for (x,z) // (x' -x)/(z'-z) = tg(a) = t // sqr(x'-x) + sqr(z'-z) = sqr(r) // z = z' - (r/(sqrt(t*t -1))) // x = x' - r(sqrt(1-(1/t*t+1))) // if( (currentTarget.hasRotation() == true) and (road.last().hasRotation == false) ) // { // qDebug() << __FUNCTION__ << "computing rotation" << road.last().pos; // float radius = 500; // float ta = tan(currentTarget.getRotation().y()); // float xx = road.last().pos.x() - radius*sqrt(1.f - (1.f/(ta*ta+1))); // float zz = road.last().pos.z() - (radius/sqrt(ta*ta+1)); // WayPoint wNew( QVec::vec3(xx,road.last().pos.y(),zz) ); // road.insert(road.end()-1,wNew); // road.last().hasRotation = true; // qDebug() << __FUNCTION__ << "after rotation" << wNew.pos << currentTarget.getRotation().y() << ta; // // } //else //qDebug() << road.last().hasRotation << road.last().pos << (road.end()-2)->pos << currentTarget.getRotation().y(); }
/** * @brief Removes points from the band if two of them are too close, ROBOT_RADIUS/3. * * @param road ... * @return void */ void ElasticBand::cleanPoints(WayPoints &road) { int i; int offset = 2; //if( road.last().hasRotation ) offset = 3; else offset = 2; for (i = 1; i < road.size() - offset; i++) // exlude 1 to avoid deleting the nextPoint and the last two to avoid deleting the target rotation { if (road[i].isVisible == false) break; WayPoint &w = road[i]; WayPoint &wNext = road[i + 1]; float dist = (w.pos - wNext.pos).norm2(); if (dist < ROAD_STEP_SEPARATION / 3.) { road.removeAt(i + 1); } } }
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; }