/**
* @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;
}
bool Sampler::checkRobotValidDirectionToTargetOneShot(const QVec & origin , const QVec & target) const
{
//qDebug() << __FUNCTION__ << "Checking between: " << origin << "and " << target;
const float MAX_LENGTH_ALONG_RAY = (target-origin).norm2();
QVec finalPoint;
float wRob=420, hRob=1600; //GET FROM INNERMODEL!!!
// if( MAX_LENGTH_ALONG_RAY < 50) //COMMENT THIS FOR NOW ::::::::::::::::::::...
// {
// qDebug() << __FUNCTION__ << "target y origin too close";
// return false;
// }
//Compute angle between origin-target line and world Zaxis
float alfa1 = QLine2D(target,origin).getAngleWithZAxis();
//qDebug() << "Angle with Z axis" << origin << target << alfa1;
// Update robot's position and align it with alfa1 so it looks at the TARGET point
innerModelSampler->updateTransformValues("robot", origin.x(), origin.y(), origin.z(), 0., alfa1, 0.);
// Compute rotation matrix between robot and world. Should be the same as alfa
QMat r1q = innerModelSampler->getRotationMatrixTo("world", "robot");
//qDebug()<< "alfa1" << alfa1 << r1q.extractAnglesR_min().y() << "robot" << innerModelSampler->transform("world","robot");
// Create a tall box for robot body with center at zero and sides:
boost::shared_ptr<fcl::Box> robotBox(new fcl::Box(wRob, hRob, wRob));
// Create a collision object
fcl::CollisionObject robotBoxCol(robotBox);
//Create and fcl rotation matrix to orient the box with the robot
const fcl::Matrix3f R1( r1q(0,0), r1q(0,1), r1q(0,2), r1q(1,0), r1q(1,1), r1q(1,2), r1q(2,0), r1q(2,1), r1q(2,2) );
//Check collision at maximum distance
float hitDistance = MAX_LENGTH_ALONG_RAY;
//Resize big box to enlarge it along the ray direction
robotBox->side = fcl::Vec3f(wRob, hRob, hitDistance);
//Compute the coord of the tip of a "nose" going away from the robot (Z dir) up to hitDistance/2
const QVec boxBack = innerModelSampler->transform("world", QVec::vec3(0, hRob/2, hitDistance/2), "robot");
//move the big box so it is aligned with the robot and placed along the nose
robotBoxCol.setTransform(R1, fcl::Vec3f(boxBack(0), boxBack(1), boxBack(2)));
//Check collision of the box with the world
for ( auto &it : restNodes)
{
if ( innerModelSampler->collide(it, &robotBoxCol))
{
//qDebug() << __FUNCTION__ << ": Robot collides with " << it;
return false;
}
}
return true;
}
//bool PlannerOMPL::computePath(const QVec& target, InnerModel* inner)
bool PlannerOMPL::computePath(const QVec& origin, const QVec &target, int maxTime)
{
//Planning proper
if (simpleSetUp == NULL)
return false;
simpleSetUp->clear();
ob::ScopedState<> start(simpleSetUp->getStateSpace());
start[0] = origin.x(); start[1] = origin.y(); start[2] = origin.z();
ob::ScopedState<> goal(simpleSetUp->getStateSpace());
goal[0] = target.x(); goal[1] = target.y(); goal[2] = target.z();
simpleSetUp->setStartAndGoalStates(start, goal);
simpleSetUp->getProblemDefinition()->print(std::cout);
currentPath.clear();
ob::PlannerStatus solved = simpleSetUp->solve(maxTime);
if (solved)
{
std::cout << __FILE__ << __FUNCTION__ << "RRT, found solution with " << simpleSetUp->getSolutionPath().getStateCount() << " waypoints" << std::endl;;
//if (simpleSetUp->haveSolutionPath())
// simpleSetUp->simplifySolution();
og::PathGeometric &p = simpleSetUp->getSolutionPath();
// simpleSetUp->getPathSimplifier()->simplify(p,5);//
// std::cout << __FILE__ << __FUNCTION__ << "Solution after simplify: " << p. getStateCount() << ". Path length: " << p.length() << std::endl;
// p.print(std::cout);
simpleSetUp->getPathSimplifier()->smoothBSpline(p);
p.interpolate();
for (std::size_t i = 0; i < p.getStateCount(); ++i)
{
currentPath.append( QVec::vec3( p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[0],
p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[1],
p.getState(i)->as<ob::RealVectorStateSpace::StateType>()->values[2]));
}
return true;
}
else
return false;
}
/**
* @brief Moves a virtual copy of the robot along the road checking for enough free space around it
*
* @param innermodel ...
* @param road ...
* @param laserData ...
* @param robotRadius ...
* @return bool
*/
bool ElasticBand::checkCollisionAlongRoad(InnerModel *innermodel, const RoboCompLaser::TLaserData& laserData, WayPoints &road, WayPoints::const_iterator robot,
WayPoints::const_iterator target, float robotRadius)
{
//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
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
QVec memo = innermodel->transform6D("world","robot");
bool free = false;
for( WayPoints::const_iterator it = robot; it != target; ++it)
{
if( it->isVisible == false)
break;
// compute orientation of the robot at the point
innermodel->updateTransformValues("robot", it->pos.x(), it->pos.y(), it->pos.z(), 0, it->rot.y(), 0);
//get Robot transformation matrix
QMat m = innermodel->getTransformationMatrix("world", "robot");
// Transform all points at one
qDebug() << __FUNCTION__ << "hello2";
m.print("m");
pointsMat.print("pointsMat");
QMat newPoints = m * pointsMat;
qDebug() << __FUNCTION__ << "hello3";
//Check if they are inside the laser polygon
for( int i=0; i<newPoints.nRows(); i++)
if( polygon.containsPoint(QPointF(pointsMat(i,0)/pointsMat(i,3), pointsMat(i,2)/pointsMat(i,3)), Qt::OddEvenFill ) == false)
{
free = false;
break;
}
free = true;
}
qDebug() << __FUNCTION__ << "hello";
// Set the robot back to its original state
innermodel->updateTransformValues("robot", memo.x(), memo.y(), memo.z(), 0, memo.ry(), 0);
return free ? true : false;
}
/**
* @brief Searches in the graph closest points to origin and target
*
* @param origin ...
* @param target ...
* @param originVertex to return selected vertex
* @param targetVertex ...
* @return void
*/
void PlannerPRM::searchClosestPoints(const QVec& origin, const QVec& target, Vertex& originVertex, Vertex& targetVertex)
{
qDebug() << __FUNCTION__ << "Searching from " << origin << "and " << target;
//prepare the query
Eigen::MatrixXi indices;
Eigen::MatrixXf distsTo;
Eigen::MatrixXf query(3,2);
indices.resize(1, query.cols());
distsTo.resize(1, query.cols());
query(0,0) = origin.x();query(1,0) = origin.y();query(2,0) = origin.z();
query(0,1) = target.x();query(1,1) = target.y();query(2,1) = target.z();
nabo->knn(query, indices, distsTo, 1);
originVertex = vertexMap.value(indices(0,0));
targetVertex = vertexMap.value(indices(0,1));
qDebug() << __FUNCTION__ << "Closest point to origin is at" << data(0,indices(0,0)) << data(1,indices(0,0)) << data(2,indices(0,0)) << " and corresponds to " << graph[originVertex].pose;
qDebug() << __FUNCTION__ << "Closest point to target is at" << data(0,indices(0,1)) << data(1,indices(0,1)) << data(2,indices(0,1)) << " and corresponds to " << graph[targetVertex].pose;
}
/** REVISARRRR
* @brief Sends the robot bakcwards on a straight line until targetT is reached.
*
* @param innerModel ...
* @param target position in World Reference System
* @return bool
*/
bool SpecificWorker::goBackwardsCommand(InnerModel *innerModel, CurrentTarget ¤t, CurrentTarget ¤tT, TrajectoryState &state, WayPoints &myRoad)
{
const float MAX_ADV_SPEED = 400.f;
const float MAX_POSITIONING_ERROR = 40; //mm
static float errorAnt = std::numeric_limits<float>::max();
///////////////////
//CHECK PARAMETERS
///////////////////
QVec target = current.getTranslation();
if (target.size() < 3 or std::isnan(target.x()) or std::isnan(target.y()) or std::isnan(target.z()))
{
qDebug() << __FUNCTION__ << "Returning. Invalid target";
RoboCompTrajectoryRobot2D::RoboCompException ex;
ex.text = "Returning. Invalid target";
throw ex;
return false;
}
state.setState("BACKWARDS");
QVec rPose = innerModel->transform("world", "robot");
float error = (rPose - target).norm2();
bool errorIncreasing = false;
if (error > errorAnt)
errorIncreasing = true;
qDebug() << __FUNCTION__ << "doing backwards" << error;
if ((error < MAX_POSITIONING_ERROR) or (errorIncreasing == true)) //TASK IS FINISHED
{
controller->stopTheRobot(omnirobot_proxy);
myRoad.requiresReplanning = true;
currentT.setWithoutPlan(true);
currentT.setState(CurrentTarget::State::GOTO);
errorAnt = std::numeric_limits<float>::max();
}
else
{
float vadv = -0.5 * error; //Proportional controller
if (vadv < -MAX_ADV_SPEED) vadv = -MAX_ADV_SPEED;
try
{ omnirobot_proxy->setSpeedBase(0, vadv, 0); }
catch (const Ice::Exception &ex)
{ std::cout << ex << std::endl; }
}
errorAnt = error;
return true;
}
///UNFiNISHED
bool Sampler::searchRobotValidStateCloseToTarget(QVec& target)
{
//If current is good, return
if( std::get<bool>(checkRobotValidStateAtTarget(target,QVec::zeros(3))) == true)
return true;
target.print("target");
//Start searching radially from target to origin and adding the vertices of a n regular polygon of radius 1000 and center "target"
const int nVertices = 12;
const float radius = 1000.f;
QVec lastPoint, minVertex, vertex;
float fi,vert;
float minDist = radius;
for(int i=0; i< nVertices; i++)
{
fi = (2.f*M_PI/nVertices) * i;
int k;
bool free;
for(k=100; k<radius; k=k+100)
{
vertex = QVec::vec3(target.x() + k*sin(fi), target.y(), target.z() + k*cos(fi));
free = std::get<bool>(checkRobotValidStateAtTarget(vertex, QVec::zeros(3)));
if (free == true)
break;
}
if( free and k < minDist )
{
minVertex = vertex;
minDist = k;
vert = fi;
}
}
if( minDist < radius)
{
target = minVertex;
target.print("new target");
qDebug() << minDist << vert;
return true;
}
else
return false;
}
std::tuple<bool, QString> Sampler::checkRobotValidStateAtTarget(const QVec &targetPos, const QVec &targetRot) const
{
QMutexLocker ml(&mutex);
QString diagnosis;
//First we move the robot in our copy of innermodel to its current coordinates
innerModelSampler->updateTransformValues("robot", targetPos.x(), targetPos.y(), targetPos.z(), targetRot.x(), targetRot.y(), targetRot.z());
///////////////////////
//// Check if the target is a point inside known space and outside forbidden regions
///////////////////////
if ( outerRegion.contains( QPointF(targetPos.x(), targetPos.z()) ) == false )
{
diagnosis += "OuterRegion " + QVariant(outerRegion).toString() + "does not contain the point";
return std::make_tuple(false, diagnosis);
}
foreach( QRectF r, innerRegions)
if( r.contains( QPointF(targetPos.x(), targetPos.z())) == true )
{
diagnosis += "InnerRegion " + QVariant(r).toString() + "contains the point";
return std::make_tuple(false, diagnosis);
}
///////////////////////
//// Check if the robot at the target collides with any know object
///////////////////////
for ( auto &in : robotNodes )
for ( auto &out : restNodes )
{
if ( innerModelSampler->collide( in, out))
{
//qDebug() << __FUNCTION__ << "collision de " << in << " con " << out;
diagnosis += "Collision of robot's mesh '" + in + "' with '" + out + "' at robot position "
+ QString::number(targetPos.x()) + ", " + QString::number(targetPos.z());
return std::make_tuple(false, diagnosis);
}
}
return std::make_tuple(true, diagnosis);
}
//////////////////
/// Main method
/////////////////
void Road::update()
{
//static QTime reloj = QTime::currentTime();
if(this->isEmpty())
return;
/// Get robot's position in world and create robot's nose
QVec robot3DPos = innerModel->transformS("world", robotname);
auto &primero = this->first();
primero = robot3DPos;
/// Robot nose
QVec noseInRobot = innerModel->transformS("world", QVec::vec3(0, 0, 1000), robotname);
//QLine2D nose = QLine2D(QVec::vec2(robot3DPos.x(), robot3DPos.z()), QVec::vec2(noseInRobot.x(), noseInRobot.z()));
QLine2D nose = QLine2D(QVec::vec2(noseInRobot.x(), noseInRobot.z()),QVec::vec2(robot3DPos.x(), robot3DPos.z()));
/// Compute closest point in road to robot. If closer than 1000mm it will use the virtual point (tip) instead of the center of the robot.
Road::iterator closestPoint = computeClosestPointToRobot(robot3DPos);
/// Compute roadTangent at closestPoint
QLine2D tangent = computeTangentAt(closestPoint);
setTangentAtClosestPoint(tangent);
/// Compute signed perpenduicular distance from robot to tangent at closest point
setRobotPerpendicularDistanceToRoad(tangent.perpendicularDistanceToPoint(robot3DPos));
/// Compute signed angle between nose and tangent at closest point
float ang = nose.signedAngleWithLine2D(tangent);
if (std::isnan(ang))
ang = 0;
setAngleWithTangentAtClosestPoint(ang);
/// Compute distance to target along trajectory
setRobotDistanceToTarget(computeDistanceToTarget(closestPoint, robot3DPos)); //computes robotDistanceVariationToTarget
setRobotDistanceVariationToTarget(robotDistanceVariationToTarget);
/// Update estimated time of arrival
setETA();
/// Compute curvature of trajectory at closest point to robot
setRoadCurvatureAtClosestPoint(computeRoadCurvature(closestPoint, 3));
/// Compute distance to last road point visible with laser field
setRobotDistanceToLastVisible(computeDistanceToLastVisible(closestPoint, robot3DPos));
/// Compute robot angle in each point
for( Road::iterator it = this->begin(); it != this->end()-1; ++it )
{
QLine2D l = computeTangentAt(it);
QVec d = l.getDirectionVector();
float ang = atan2(d.x(), d.y());
if(ang>0) ang -= M_PI;
else ang += M_PI;
it->rot = QVec::vec3(0, ang, 0);
}
/// Check for arrival to target (translation) TOO SIMPLE
// if (((((int) getIndexOfCurrentPoint()+1 == (int) this->size()) and (getRobotDistanceToTarget() < threshold))) or
// (((int) getIndexOfCurrentPoint()+1 == (int) this->size()) and (getRobotDistanceVariationToTarget() > 0)))
// {
// qDebug() << "Road::" <<__FUNCTION__ << "ROAD: FINISHED";
// qDebug() << "Road::" <<__FUNCTION__ << " reason: " << ((int) getIndexOfCurrentPoint()+1 == (int)this->size()) << "index " << getIndexOfCurrentPoint();
// qDebug() << "Road::" <<__FUNCTION__ << " reason: " << (getRobotDistanceToTarget() < threshold) << " distance: "<<getRobotDistanceToTarget() << getRobotDistanceVariationToTarget();
//
// clear();
// setFinished(true);
// }
// else
{
///////////////////////////////////////////
//Check for blocked road
///////////////////////////////////////////
// qDebug() << "Road::" <<__FUNCTION__ << "ROAD: Robot distance to last visible" << getRobotDistanceToLastVisible() << (getIterToLastVisiblePoint() < this->end());
//print();
if( getRobotDistanceToLastVisible() < 250 and //PARAMS
getIterToLastVisiblePoint() < this->end())
{
qDebug() <<"Road::" <<__FUNCTION__ <<"BLOCKED" << "distanceToLastVisible" << getRobotDistanceToLastVisible();
setBlocked(true);
}
else
setBlocked(false);
}
//printRobotState(innerModel);
// print();
}
bool SpecificWorker::checkFreeWay( const QVec &targetInRobot)
{
qDebug() << __FUNCTION__ ;
//First turn the robot to point towards the new target
if (alignToTarget( targetInRobot ) != true )
{
stopAction();
qFatal("Could not align the robot");
}
QList<QVec> lPoints;
//points on the corners of thw square
lPoints.append( QVec::vec3(ROBOT_RADIUS,0,ROBOT_RADIUS));
lPoints.append( QVec::vec3(ROBOT_RADIUS,0,-ROBOT_RADIUS));
lPoints.append( QVec::vec3(-ROBOT_RADIUS,0,-ROBOT_RADIUS));
lPoints.append( QVec::vec3(-ROBOT_RADIUS,0,ROBOT_RADIUS));
//points on the sides
lPoints.append( QVec::vec3(ROBOT_RADIUS,0,0));
lPoints.append( QVec::vec3(0,0,-ROBOT_RADIUS));
lPoints.append( QVec::vec3(-ROBOT_RADIUS,0,0));
lPoints.append( QVec::vec3(0,0, ROBOT_RADIUS));
float dist = targetInRobot.norm2();
float alpha =atan2(targetInRobot.x(), targetInRobot.z() );
float step = ceil(dist/ (ROBOT_SIZE/3));
QVec tNorm = targetInRobot.normalize();
QVec r;
inner->updateTransformValues ("vbox",0, 0, 0, 0, 0, 0, "robot");
for(size_t i = 0; i<ldata.size(); i++)
{
if(ldata[i].angle <= alpha)
{
for(float landa=400; landa<=dist; landa+=step)
{
r = tNorm * landa;
inner->updateTransformValues ("vbox",r.x(), r.y(), r.z(), 0, alpha, 0, "robot");
foreach(QVec p, lPoints)
{
QVec pointInRobot = inner->transform("robot", p, "vbox");
float dPR = pointInRobot.norm2();
float alphaPR = atan2(pointInRobot.x(), pointInRobot.z());
for(auto ld : ldata)
if(ld.angle <= alphaPR)
{
if( ld.dist>0 and ld.dist < LASER_MAX and ld.dist >= dPR)
{
//qDebug()<<__FUNCTION__<< "Hay camino libre al target" << ldata[i].dist << d;
break;
}
else
{
qDebug() << "collision of point(R) " << inner->transform("robot", p, "vbox");
return false;
}
}
}
}
qDebug()<<__FUNCTION__<< "Hay camino libre al target. Laser distance:" << ldata[i].dist << ". Target distance:" << dist;
break; //We found the laser ray aligned with the target.
}
}
//////////////////
/// Main method
/////////////////
void WayPoints::update()
{
//////////////////////////////////////////////////////
//Get robot's position in world and create robot's nose
//////////////////////////////////////////////////////
QVec robot3DPos = innerModel->transform("world", "robot");
QVec noseInRobot = innerModel->transform("world", QVec::vec3(0, 0, 1000), "robot");
QLine2D nose = QLine2D(QVec::vec2(robot3DPos.x(), robot3DPos.z()), QVec::vec2(noseInRobot.x(), noseInRobot.z()));
////////////////////////////////////////////////////
//Compute closest point in road to robot. If closer than 1000mm it will use the virtual point (tip) instead of the center of the robot.
///////////////////////////////////////////////////
// if (getRobotDistanceToTarget() < 1000)
// {
// robot3DPos = innerModel->transform("world", "virtualRobot");
// }
WayPoints::iterator closestPoint = computeClosestPointToRobot(robot3DPos);
///////////////////////////////////////
//Compute roadTangent at closestPoint
///////////////////////////////////////
QLine2D tangent = computeTangentAt(closestPoint);
setTangentAtClosestPoint(tangent);
//////////////////////////////////////////////////////////////////////////////
//Compute signed perpenduicular distance from robot to tangent at closest point
///////////////////////////////////////////////////////////////////////////////
setRobotPerpendicularDistanceToRoad(tangent.perpendicularDistanceToPoint(robot3DPos));
////////////////////////////////////////////////////
//Compute signed angle between nose and tangent at closest point
////////////////////////////////////////////////////
float ang = nose.signedAngleWithLine2D(tangent);
if (std::isnan(ang))
ang = 0;
setAngleWithTangentAtClosestPoint(ang);
/////////////////////////////////////////////
//Compute distance to target along trajectory
/////////////////////////////////////////////
setRobotDistanceToTarget(computeDistanceToTarget(closestPoint, robot3DPos)); //computes robotDistanceVariationToTarget
setRobotDistanceVariationToTarget(robotDistanceVariationToTarget);
//////////////////////////////////
//Update estimated time of arrival
//////////////////////////////////
setETA();
////////////////////////////////////////////////////////////
//Compute curvature of trajectory at closest point to robot
////////////////////////////////////////////////////////////
setRoadCurvatureAtClosestPoint(computeRoadCurvature(closestPoint, 3));
////////////////////////////////////////////////////////////
//Compute distance to last road point visible with laser field
////////////////////////////////////////////////////////////
setRobotDistanceToLastVisible(computeDistanceToLastVisible(closestPoint, robot3DPos));
////////////////////////////////////////////////////////////
// Compute robot angle in each point
// //////////////////////////////////////////////////////////
for( WayPoints::iterator it = this->begin(); it != this->end(); ++it )
{
QLine2D l = computeTangentAt(it);
QVec d = l.getDirectionVector();
float ang = atan2(d.x(), d.y());
if(ang>0) ang -= M_PI;
else ang += M_PI;
it->rot = QVec::vec3(0, ang, 0);
}
//////////////////////////////////////////////////////////
//Check for arrival to target (translation) TOO SIMPLE
/////////////////////////////////////////////////////////
threshold = 20; //////////////////////////////////////////////////FIX THIS
//qDebug() << __FUNCTION__ << "Arrived:" << getRobotDistanceToTarget() << this->threshold << getRobotDistanceVariationToTarget();
if (((((int) getIndexOfCurrentPoint() + 1 == (int) this->size()) or (getRobotDistanceToTarget() < threshold))) or
((getRobotDistanceToTarget() < 100) and (getRobotDistanceVariationToTarget() > 0)))
{
qDebug() << __FUNCTION__ << "FINISHED";
setFinished(true);
}
else
{
///////////////////////////////////////////
//Check for blocked road
///////////////////////////////////////////
qDebug() << __FUNCTION__ << "ROAD: Robot distance to last visible" << getRobotDistanceToLastVisible();
//print();
/* if( getRobotDistanceToLastVisible() < 150 and getIterToLastVisiblePoint() < this->end())
setBlocked(true);
else
setBlocked(false);*/
}
}
QVec SpecificWorker::movFoottoPoint(QVec p, bool &exito)
{
QVec angles=QVec::zeros(3);
StateLeg s=getStateLeg();
double q1=s.posclavicula,
q2=s.poshombro*signleg,
q3=s.posclavicula*signleg;
double x=p.x(), y=p.y(), z=p.z(),
r=abs(sqrt(pow(x,2)+pow(z,2))-coxa),
cosq3=(pow(r,2)+pow(y,2)-pow(tibia,2)-pow(femur,2))/(2*tibia*femur);
if(cosq3>1)
cosq3=1;
else if(cosq3<-1)
cosq3=-1;
double senq3=-sqrt(1-pow(cosq3,2));
if(senq3>1)
senq3=1;
else if(senq3<-1)
senq3=-1;
double L=sqrt(pow(y,2)+pow(r,2));
if(L<tibia+femur &&( x>0 || z>0)){
q1=atan2(x,z);
q3=atan2(senq3,cosq3);
q2=atan2(y,r)-atan2((tibia*senq3),(femur+(tibia*cosq3)));
// if(q1<motorsparams[motores.at(0).toStdString()].maxPos && q1>motorsparams[motores.at(0).toStdString()].minPos &&
// q2<motorsparams[motores.at(1).toStdString()].maxPos && q2>motorsparams[motores.at(2).toStdString()].minPos &&
// q3<motorsparams[motores.at(2).toStdString()].maxPos && q3>motorsparams[motores.at(2).toStdString()].minPos)
// {
q2 = q2 + 0.22113;
q3 = q3 + 0.578305;
exito=true;
// }
// else
// {
// qDebug()<<"Posicion imposible";
// exito=false;
// }
}
else
{
qDebug()<<"Posicion imposible";
exito=false;
}
//----------------------------------------------------------------------------
/*
KDL::Chain chain;
//a alfa d theta
chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotY),KDL::Frame(KDL::Vector(0.0,0.0,1))));
chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotX),KDL::Frame(KDL::Vector(0.0,0.0,1))));
chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotX),KDL::Frame(KDL::Vector(0.0,0.0,1))));
KDL::ChainFkSolverPos_recursive fksolver(chain);//objeto para calcular la cinemática directa del robot
KDL::ChainIkSolverVel_pinv iksolver1v(chain);//objeto para calcular la cinemática inversa
KDL::ChainIkSolverPos_NR iksolver1(chain,fksolver,iksolver1v,100,1e-6);//como máximo 100 iteraciones, parar si el error es menor que 1e-6
KDL::JntArray q(chain.getNrOfJoints());
KDL::JntArray q_init(chain.getNrOfJoints());
KDL::Frame F_dest;
F_dest.p= KDL::Vector(p.x(),p.y(),p.z());
F_dest.M= KDL::Rotation(1,0,0,
0,1,0,
0,0,1);
int ret = iksolver1.CartToJnt(q_init,F_dest,q);*/
angles(0)=q1/*+motorsparams[motores.at(0).toStdString()].offset*/;
angles(1)=q2/*+motorsparams[motores.at(1).toStdString()].offset*/;
angles(2)=q3/*+motorsparams[motores.at(2).toStdString()].offset*/;
return angles;
}
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;
}
bool SpecificWorker::rotar()
{
static bool estado=true;
if(lrot!=QVec::vec3(0, 0, 0))
{
static float i=0;
bool ismoving=false;
for(int k=0;k<6;k++)
if(proxies[k]->getStateLeg().ismoving)
{
ismoving=true;
break;
}
if(!ismoving)
{
if(i==0)
for(int i=0;i<6;i++)
pre_statelegs[i]=statelegs[i];
QVec ini,fin;
for(int j=0;j<6;j++)
{
RoboCompLegController::StateLeg st=pre_statelegs[j];
QVec posini =QVec::vec3(st.x,legsp[j].y(),st.z);
if((j==2 || j==3))
{
ini = posini;
if(estado)
fin = legsp[j]+lrot;
else
fin = legsp[j]-lrot;
}
else
{
ini = posini;
if(estado)
fin = legsp[j]-lrot;
else
fin = legsp[j]+lrot;
}
QVec tmp;
if(estado)
if(j==0||j==3||j==4)
tmp=bezier3(ini,bezier2(ini,fin,0.5)+lmed,fin,i);
else
tmp = bezier2(ini,fin,i);
else
if(j==0||j==3||j==4)
tmp = bezier2(ini,fin,i);
else
tmp=bezier3(ini,bezier2(ini,fin,0.5)+lmed,fin,i);
RoboCompLegController::PoseLeg p;
p.x = tmp.x();
p.y = tmp.y();
p.z = tmp.z();
p.ref = base.toStdString();
p.vel = 6;
proxies[j]->setIKLeg(p,false);
}
i+=tbezier;
if (i>1)
{
i=0;
estado=!estado;
return true;
}
}
return false;
}
else
return true;
}
//NOT WORKING WELL
bool Sampler::checkRobotValidDirectionToTargetBinarySearch(const QVec & origin , const QVec & target, QVec &lastPoint) const
{
const float MAX_LENGTH_ALONG_RAY = (target-origin).norm2();
bool hit = false;
QVec finalPoint;
float wRob=600, hRob=1600; //GET FROM INNERMODEL!!!
if( MAX_LENGTH_ALONG_RAY < 50) //FRACTION OF ROBOT SIZE
{
qDebug() << __FUNCTION__ << "target y origin too close";
lastPoint = target;
return false;
}
//Compute angle between origin-target line and world Zaxis
float alfa1 = QLine2D(target,origin).getAngleWithZAxis();
//qDebug() << "Angle with Z axis" << origin << target << alfa1;
// Update robot's position and align it with alfa1 so it looks at the TARGET point
innerModelSampler->updateTransformValues("robot", origin.x(), origin.y(), origin.z(), 0., alfa1, 0.);
// Compute rotation matrix between robot and world. Should be the same as alfa
QMat r1q = innerModelSampler->getRotationMatrixTo("world", "robot");
// Create a tall box for robot body with center at zero and sides:
boost::shared_ptr<fcl::Box> robotBox(new fcl::Box(wRob, hRob, wRob));
// Create a collision object
fcl::CollisionObject robotBoxCol(robotBox);
//Create and fcl rotation matrix to orient the box with the robot
const fcl::Matrix3f R1( r1q(0,0), r1q(0,1), r1q(0,2), r1q(1,0), r1q(1,1), r1q(1,2), r1q(2,0), r1q(2,1), r1q(2,2) );
//Check collision at maximum distance
float hitDistance = MAX_LENGTH_ALONG_RAY;
//Resize big box to enlarge it along the ray direction
robotBox->side = fcl::Vec3f(wRob, hRob, hitDistance);
//Compute the coord of the tip of a "nose" going away from the robot (Z dir) up to hitDistance/2
const QVec boxBack = innerModelSampler->transform("world", QVec::vec3(0, hRob/2, hitDistance/2.), "robot");
//move the big box so it is aligned with the robot and placed along the nose
robotBoxCol.setTransform(R1, fcl::Vec3f(boxBack(0), boxBack(1), boxBack(2)));
//Check collision of the box with the world
for (uint out=0; out<restNodes.size(); out++)
{
hit = innerModelSampler->collide(restNodes[out], &robotBoxCol);
if (hit) break;
}
//Binary search. If not free way do a binary search
if (hit)
{
hit = false;
float min=0;
float max=MAX_LENGTH_ALONG_RAY;
while (max-min>10)
{
//set hitDistance half way
hitDistance = (max+min)/2.;
// Stretch and create the stick
robotBox->side = fcl::Vec3f(wRob,hRob,hitDistance);
const QVec boxBack = innerModelSampler->transform("world", QVec::vec3(0, hRob/2, hitDistance/2.), "robot");
robotBoxCol.setTransform(R1, fcl::Vec3f(boxBack(0), boxBack(1), boxBack(2)));
//qDebug() << "checking ang" << r1q.extractAnglesR_min().y() << "and size " << boxBack << hitDistance;
// Check collision using current ray length
for (uint out=0; out<restNodes.size(); out++)
{
hit = innerModelSampler->collide(restNodes[out], &robotBoxCol);
if (hit)
break;
}
// Manage next min-max range
if (hit)
max = hitDistance;
else
min = hitDistance;
}
// Set final hit distance
hitDistance = (max+min)/2.;
if( hitDistance < 50)
lastPoint = origin;
else
lastPoint = innerModelSampler->transform("world", QVec::vec3(0, 0, hitDistance-10), "robot");
return false;;
}
else //we made it up to the Target!
{
lastPoint = target;
return true;
}
}
float SpecificWorker::goReferenced(const TargetPose &target_, const float xRef, const float zRef, const float threshold)
{
/////////////////////
//CHECK PARAMETERS
/////////////////////
if (isnan(target_.x) or std::isnan(target_.y) or std::isnan(target_.z) or std::isnan(target_.ry))
{
qDebug() << __FUNCTION__ << "Returning. Input parameter -target- is not valid:" << target_.x << target_.y
<< target_.z << target_.ry;
RoboCompTrajectoryRobot2D::RoboCompException ex;
ex.text = "Doing nothing. Invalid Target with nan in it";
throw ex;
}
QVec currentT = currentTarget.getTranslation();
QVec currentRot = currentTarget.getRotation();
QVec robotT = innerModel->transform("world", "robot");
QVec robotRot = innerModel->transform6D("world", "robot").subVector(3, 5);
QVec targetT = QVec::vec3(target_.x, target_.y, target_.z);
QVec targetRot = QVec::vec3(target_.rx, target_.ry, target_.rz);
drawTarget(targetT);
qDebug() << __FUNCTION__
<< "----------------------------------------------------------------------------------------------";
qDebug() << __FUNCTION__ << ": Target received" << targetT << targetRot << "with ROBOT at" << robotT << robotRot;
qDebug() << __FUNCTION__ << "Translation error: " << (targetT - robotT).norm2() << "mm. Rotation error:"
<< targetRot.y() - robotRot.y() << "rads";
///////////////////////////////////////////////
//Maximun admitted rate of requests (one each 250 ms)
///////////////////////////////////////////////
if (relojForInputRateControl.elapsed() < 250)
{
RoboCompTrajectoryRobot2D::RoboCompException ex;
ex.text = "Fail. Call too close in time. Please wait and call again";
throw ex;
}
else
relojForInputRateControl.restart();
///////////////////////////////////////////////
// Minimun change admitted 60mm and 0.05rads
///////////////////////////////////////////////
const int MINCHANGE = 7;
const float MINROTATION = 0.03;
if ((targetT - robotT).norm2() < MINCHANGE and fabs(targetRot.y() - robotRot.y()) < MINROTATION)
{
QString s = "Fail. Target too close to current pose. TDist=" + QString::number((targetT - robotT).norm2()) +
"mm. Min = 60mm. RDist="
+ QString::number(targetRot.y() - robotRot.y()) + "rads. Min = 0.05rads";
qDebug() << __FUNCTION__ << s;
RoboCompTrajectoryRobot2D::RoboCompException ex;
ex.text = s.toStdString();
tState.setDescription("Target too close");
throw ex;
}
///////////////////////////////////////////////
// Check if robot is inside limits and not in collision.
///////////////////////////////////////////////
auto r = sampler.checkRobotValidStateAtTarget(robotT, robotRot);
if (std::get<bool>(r) == false)
{
RoboCompTrajectoryRobot2D::RoboCompException ex;
ex.text = "Execption: Robot is outside limits or colliding. Ignoring request. " + std::get<QString>(r).toStdString();
qDebug() << __FUNCTION__ << QString::fromStdString(ex.text);
tState.setDescription(ex.text);
currentTarget.setState(CurrentTarget::State::ROBOT_COLLISION);
throw ex;
}
///////////////////////////////////////////////
// Check if target is a a valid point, inside limits and not in collision.
///////////////////////////////////////////////
r = sampler.checkRobotValidStateAtTarget(targetT, targetRot);
if (std::get<bool>(r) == false)
{
RoboCompTrajectoryRobot2D::RoboCompException ex;
ex.text = "Exception: Target outside limits or colliding. Ignoring request. " + std::get<QString>(r).toStdString();
qDebug() << __FUNCTION__ << QString::fromStdString(ex.text);
tState.setDescription(ex.text);
currentTarget.setState(CurrentTarget::State::TARGET_COLLISION);
throw ex;
}
///////////////////////////////////////////////
// Let's see what these guys want
///////////////////////////////////////////////
// move virtualrobot to xref, zRef to act as a surrogate
//innerModel->updateTransformValues("virtualRobot", xRef, 0, zRef, 0, 0, 0, "robot");
//InnerModelDraw::addPlane_ignoreExisting(innerViewer, "virtualRobot", "robot", QVec::vec3(xRef,0,zRef), QVec::vec3(0,0,0), "#555555", QVec::vec3(50,1000,50));
road.setThreshold(threshold);
currentTarget.reset(targetT, targetRot, target_.doRotation);
currentTarget.setState(CurrentTarget::State::GOTO);
tState.setState("EXECUTING");
taskReloj.restart();
return (robotT - targetT).norm2();
}
/**
* @brief Computes de angle (-PI, PI) between this and endpoint p wrt the Y axis. If both points are the same, the method returns 0.
*
* @param p end point of segment
* @return angle
**/
RMat::T RMat::QVec::angleOf2DSegment( const QVec & p) const
{
Q_ASSERT_X( size() == 2 and p.size()==2 , "QVec::angleOf2DSegment", "incorrect size of parameters");
return atan2( p.x()-operator[](0), p.y()-operator[](1) );
}
bool SpecificWorker::Quadruped()
{
static float i=0;
bool ismoving=false;
if(lini!=QVec::vec3(0, 0, 0)&&lfin!=QVec::vec3(0,0,0))
{
//Comprueba que alguna pata se mueva
for(int k=0;k<6;k++)
if(proxies[k]->getStateLeg().ismoving){
ismoving=true;
break;
}
if(!ismoving)
{
//patas por arco
for(int s=0;s<2;s++)
{
RoboCompLegController::StateLeg st=posiciones[legsQuadrupedOn[s]];
QVec posini = QVec::vec3(st.x,legCoord[legsQuadrupedOn[s]].y(),st.z);
QVec ini = posini,fin = legCoord[legsQuadrupedOn[s]]+lfin,med=legCoord[legsQuadrupedOn[s]];
QVec tmp = bezier3(ini,QVec::vec3(med.x(),0,med.z()),fin,i);
RoboCompLegController::PoseLeg p;
p.x=tmp.x();
p.y=tmp.y();
p.z=tmp.z();
p.ref=base.toStdString();
p.vel=6;
proxies[legsQuadrupedOn[s]]->setIKLeg(p,false);
}
// patas por tierra
for(int s=0;s<4;s++)
{
RoboCompLegController::StateLeg st=posiciones[legsQuadrupedOff[s]];
QVec posini =QVec::vec3(st.x,legCoord[legsQuadrupedOff[s]].y(),st.z);
lini = QVec::vec3(-X,0,-Z/3);
QVec ini = posini,fin = legCoord[legsQuadrupedOff[s]]+lini;
QVec tmp=bezier2(ini,fin,i);
RoboCompLegController::PoseLeg p;
p.x=tmp.x();
p.y=tmp.y();
p.z=tmp.z();
p.ref=base.toStdString();
p.vel=6;
proxies[legsQuadrupedOff[s]]->setIKLeg(p,false);
}
i+=.1;
if (i>1)
{
int aux[]={legsQuadrupedOn[0],legsQuadrupedOn[1]};
legsQuadrupedOn[0]=legsQuadrupedOff[0];
legsQuadrupedOn[1]=legsQuadrupedOff[1];
legsQuadrupedOff[0]=legsQuadrupedOff[2];
legsQuadrupedOff[1]=legsQuadrupedOff[3];
legsQuadrupedOff[2]=aux[0];
legsQuadrupedOff[3]=aux[1];
i=0;
return true;
}
}
return false;
}
return true;
}