void SpecificWorker::getMarcas()
{
QVec robotInWorld = innermodel->transform("world","robot");
listaMarcas = getapriltags_proxy->checkMarcas();
int tag;
if (!boxPicked){
tag = getMinTag(0);
if (tag != -1){ //If box found
QVec targetRobot = innermodel->transform("world",QVec::vec3(listaMarcas[tag].tx, 0, listaMarcas[tag].tz) , "rgbd");
T.insertTag(currentBox, targetRobot.x(), targetRobot.z());
}
}
else{
tag = getTag(currentTag);
if ( tag != -1){
QVec targetRobot = innermodel->transform("world",QVec::vec3(listaMarcas[tag].tx, 0, listaMarcas[tag].tz) , "rgbd");
T.insertTag(listaMarcas[tag].id, targetRobot.x(), targetRobot.z());
}
}
}
/**
* @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 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;
}
void SpecificWorker::irASubTarget()
{
qDebug() << __FUNCTION__<<"ir a subTarget";
QVec t = inner->transform ( "robot", ctarget.subTarget, "world" );
float alpha =atan2 ( t.x(), t.z() );
float r= 0.4*alpha;
float d = t.norm2();
if ( d<100 ) {
ctarget.activeSub=false;
differentialrobot_proxy->setSpeedBase ( 0,0 );
sleep ( 1 );
} else {
if ( fabs ( r ) > 0.2 ) {
d = 0;
}
if ( d>300 ) {
d=300;
}
try {
differentialrobot_proxy->setSpeedBase ( d,r );
} catch ( const Ice::Exception &ex ) {
std::cout << ex << std::endl;
}
}
}
bool SpecificWorker::hayCamino()
{
QVec t = inner->transform("robot", ctarget.target, "world");
float alpha =atan2(t.x(), t.z() );
float d = t.norm2();
float x, z;
//int i = 50;
for ( uint i = 0; i<ldata.size(); i++ ) {
if ( ldata[i].angle <= alpha ) {
if ( ldata[i].dist < d ) {
qDebug() <<"NO hay camino";
return false;
} else {
ctarget.activeSub=false;
qDebug() <<"hay camino";
return true;
}
}
}
qDebug() <<"NO ve la marca";
state = State::TURN;
return false;
}
void SpecificWorker::compute()
{
try {
differentialrobot_proxy->getBaseState ( bState );
ldata = laser_proxy->getLaserData();
inner->updateTransformValues ( "robot", bState.x, 0, bState.z, 0, bState.alpha, 0 );
float alpha;
QVec t;
switch ( state ) {
case State::INIT:
state = State::IDLE;
break;
case State::IDLE:
break;
case State::WORKING:
if ( heLlegado() ) {
qDebug() << __FUNCTION__<< "Arrived to target" << ctarget.target;
stopRobot();
state = State::FINISH;
} else if ( hayCamino() ) {
irATarget();
}
break;
case State::TURN:
qDebug() << "Buscando punto" << ctarget.target;
t = inner->transform ( "robot", ctarget.target, "world" );
alpha =atan2 ( t.x(), t.z() );
if ( alpha <= ldata.front().angle and alpha >= ldata. back().angle ) {
stopRobot();
state = State::WORKING;
} else
try {
differentialrobot_proxy->setSpeedBase ( 0, 0.4 );
} catch ( Ice::Exception &ex ) {
std::cout<<ex.what() <<std::endl;
};
break;
case State::FINISH:
sleep ( 2 );
undrawTarget ( "target" );
state = State::IDLE;
break;
}
} catch ( const Ice::Exception &e ) {
std::cout << "Error reading from Camera" << e << std::endl;
}
//histogram();
innerViewer->update();
osgView->autoResize();
osgView->frame();
}
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;
}
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);
}
/**
* @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;
}
/**
* \brief Default constructor
*/
SpecificWorker::SpecificWorker(MapPrx& mprx) : GenericWorker(mprx)
{
inner = new InnerModel("/home/salabeta/Robotica2015/RoCKIn@home/world/apartment.xml");
//Set odometry for initial robot TargetPose
try {
differentialrobot_proxy->getBaseState ( bState );
qDebug() << __FUNCTION__<< bState.x << bState.z << bState.alpha;
try {
inner->transform ( "world",QVec::zeros ( 6 ),"initialRobotPose" );
if ( bState.x == 0 and bState.z == 0 ) { //RCIS just initiated. We change robot odometry to the initialRobotPose
QVec rpos = inner->transform ( "world", QVec::zeros ( 6 ),"robot" );
RoboCompDifferentialRobot::TBaseState bs;
bs.x=rpos.x();
bs.z=rpos.z();
bs.alpha=rpos.ry();
differentialrobot_proxy->setOdometer ( bs );
qDebug() << "Robot odometry set to" << rpos;
} else {
inner->updateTransformValues ( "initialRobotPose", 0,0,0,0,0,0 );
}
} catch ( std::exception &ex ) {
std::cout<<ex.what() <<std::endl;
};
} catch ( Ice::Exception &ex ) {
std::cout<<ex.what() <<std::endl;
};
qDebug() << __FUNCTION__<< bState.x << bState.z << bState.alpha;
graphicsView->setScene ( &scene );
graphicsView->show();
graphicsView->scale ( 3,3 );
//Innermodelviewer
osgView = new OsgView ( this );
osgGA::TrackballManipulator *tb = new osgGA::TrackballManipulator;
osg::Vec3d eye ( osg::Vec3 ( 4000.,4000.,-1000. ) );
osg::Vec3d center ( osg::Vec3 ( 0.,0.,-0. ) );
osg::Vec3d up ( osg::Vec3 ( 0.,1.,0. ) );
tb->setHomePosition ( eye, center, up, true );
tb->setByMatrix ( osg::Matrixf::lookAt ( eye,center,up ) );
osgView->setCameraManipulator ( tb );
innerViewer = new InnerModelViewer ( inner, "root", osgView->getRootGroup(), true );
show();
}
///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;
}
void RCDraw::draw3DRoiOnFloor(const QVec & center, const QMat & cov, const QColor & col, bool fill, int id)
{
//extract submatrix X;Z for 2D floor porjection
QMat cov2D(2,2);
/*cov2D(0,0) = cov(0,0);
cov2D(0,1) = cov(0,2);
cov2D(1,0) = cov(2,0);
cov2D(1,1) = cov(2,2);
*///cov2D.print("cov2D");
cov2D = cov;
QVec vals(2);
QMat vecs = cov2D.eigenValsVectors ( vals );
float sigma1 = vals(0);
float sigma2 = vals(1);
float phi;
if (sigma1 > sigma2)
phi = atan2(vecs(1,0),vecs(0,0));
else
phi = atan2(vecs(1,1),vecs(0,1));
float ang = phi*180/M_PI + (M_PI/2);
qDebug() << "ang " << phi;
/* if (phi<0)
ang = phi*180/M_PI + 360;
else
ang = phi*180/M_PI;*/
if ( isnan(sigma1)==false and isinf(sigma1)==false and isnan(sigma2)==false and isinf(sigma2)==false)
{
TEllipse e;
e.center.setX( center.x());
e.center.setY( center.z());
e.rx = sigma1;
e.ry = sigma2;
e.color = col;
e.id = id;
e.fill = fill;
e.ang = ang;
ellipseQueue.enqueue ( e );
}
}
void SpecificWorker::irATarget()
{
QVec t = inner->transform ( "robot", ctarget.target, "world" );
qDebug() << __FUNCTION__<< ctarget.target;
qDebug() << __FUNCTION__<< t;
float alpha =atan2 ( t.x(), t.z() );
float r= 0.3*alpha;
float d = 0.3*t.norm2();
qDebug() << "velocidad " << d;
if ( fabs ( r ) > 0.2 ) {
d = 0;
}
if ( d>300 ) {
d=300;
}
try {
differentialrobot_proxy->setSpeedBase ( d,r );
} catch ( const Ice::Exception &ex ) {
std::cout << ex << std::endl;
}
}
void RCDrawRobot::drawRobot(InnerModel * innerModel, const QColor &color)
{
QVec p1 (3);
int radio = 320;
//Body
QVec geomCenter = innerModel->transform("world", QVec::vec3(0,0,0), "robotGeometricCenter");
QVec leftWheelP1 = innerModel->transform("world", QVec::vec3(-radio-50, 0, 75), "base");
QVec leftWheelP2 = innerModel->transform("world", QVec::vec3(-radio-50, 0, -75), "base");
QVec headRobot = innerModel->transform("world", QVec::vec3(0, 0, radio), "base" );
QLine leftWheel(QPoint(leftWheelP1.x(), leftWheelP1.z()),QPoint(leftWheelP2.x(),leftWheelP2.z()));
QVec rightWheelP1 = innerModel->transform("world", QVec::vec3(radio+50, 0, 75), "base");
QVec rightWheelP2 = innerModel->transform("world", QVec::vec3(radio+50, 0, -75), "base");
QLine rightWheel(QPoint(rightWheelP1.x(), rightWheelP1.z()),QPoint(rightWheelP2.x(),rightWheelP2.z()));
QMat m = innerModel->getRotationMatrixTo("world", "base");
drawSquare( QPointF( (int) rint(geomCenter(0)), (int) rint(geomCenter(2)) ), 2.*radio, 2.*radio, color, true, -1, atan2f(m(2,0),m(0,0)));
drawSquare( QPointF( headRobot.x(), headRobot.z() ), radio/3, radio/3, Qt::magenta, true );
// drawEllipse( QPointF( innerModel->getBaseX(), innerModel->getBaseZ() ), radio/6, radio/6, Qt::magenta, true );
//Wheels
drawLine( leftWheel, Qt::black, 60);
drawLine( rightWheel, Qt::black, 60);
// drawLeftFieldOfView(innerModel);
// drawRightFieldOfView(innerModel);
if((geomCenter(0)-visibleCenter(0))<win.x())
visibleCenter(0)=geomCenter(0);
if((geomCenter(0)-visibleCenter(0))>win.x()+win.width())
visibleCenter(0)=geomCenter(0);
if((geomCenter(2)-visibleCenter(1))<-win.y())
visibleCenter(1)=geomCenter(2);
if((geomCenter(2)-visibleCenter(1))>-win.y()-win.height())
visibleCenter(1)=geomCenter(2);
}
//////////////////
/// 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);*/
}
}
//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;
}
}
void SpecificWorker::action_HandObject(bool newAction)
{
// Get symbols' map
std::map<std::string, AGMModelSymbol::SPtr> symbols;
try
{
symbols = worldModel->getSymbolsMap(params/*, "robot", "room", "object", "status"*/); //ALL THE SYMBOLS GIVEN IN THE RULE
}
catch(...)
{
printf("navigationAgent, action_HandObject: Couldn't retrieve action's parameters\n");
printf("<<WORLD\n");
AGMModelPrinter::printWorld(worldModel);
printf("WORLD>>\n");
if (worldModel->size() > 0) { exit(-1); }
}
// Get target
int roomID, personID, robotID;
try
{
if (symbols["room"].get() and symbols["person"].get() and symbols["robot"].get())
{
roomID = symbols["room"]->identifier; //7 ROOM
personID =symbols["person"]->identifier;// PERSON
robotID = symbols["robot"]->identifier; //1 ROBOT
}
else
{
printf("navigationAgent, action_HandObject: parameters not in the model yet\n");
return;
}
}
catch(...)
{
printf("navigationAgent, action_HandObject ERROR: SYMBOL DOESN'T EXIST \n");
exit(2);
}
// GET THE INNERMODEL NAMES OF TH SYMBOLS
QString robotIMID;
QString roomIMID;
QString personIMID;
try
{
robotIMID = QString::fromStdString(worldModel->getSymbol(robotID)->getAttribute("imName"));
roomIMID = QString::fromStdString(worldModel->getSymbol(roomID)->getAttribute("imName"));
//we need to obtain the imName of the torso node. TrackingId+"XN_SKEL_TORSO"
QString trackingId= QString::fromStdString(worldModel->getSymbol(personID)->getAttribute("TrackingId"));
personIMID = trackingId +"XN_SKEL_TORSO";
}
catch(...)
{
printf("navigationAgent, action_HandObject: ERROR IN GET THE INNERMODEL NAMES\n");
qDebug()<<"[robotIMID"<<robotIMID<<"roomIMID"<<roomIMID<<"personIMID"<<personIMID<<"]";
exit(2);
}
// GET THE TARGET POSE:
RoboCompTrajectoryRobot2D::TargetPose tgt;
try
{
if (not (innerModel->getNode(roomIMID) and innerModel->getNode(personIMID))) return;
QVec poseInRoom = innerModel->transform6D(roomIMID, personIMID); // FROM OBJECT TO ROOM
qDebug()<<"[robotIMID"<<robotIMID<<"roomIMID"<<roomIMID<<"personIMID"<<personIMID<<"]";
qDebug()<<" TARGET POSE: "<< poseInRoom;
tgt.x = poseInRoom.x();
tgt.y = 0;
tgt.z = poseInRoom.z();
tgt.rx = 0;
tgt.ry = poseInRoom.ry();
tgt.rz = 0;
tgt.doRotation = false;
}
catch (...)
{
qDebug()<<"navigationAgent, action_HandObject: innerModel exception";
}
try
{
// if (!haveTarget)
{
try
{
QVec graspRef = innerModel->transform("robot", "shellyArm_grasp_pose");
float th=20;
go(tgt.x, tgt.z, tgt.ry, tgt.doRotation, graspRef.x(), graspRef.z(), th);
std::cout << "trajectoryrobot2d->go(" << currentTarget.x << ", " << currentTarget.z << ", " << currentTarget.ry << ", " << currentTarget.doRotation << ", " << graspRef.x() << ", " << graspRef.z() << " )\n";
haveTarget = true;
}
catch(const Ice::Exception &ex)
{
std::cout <<"navigationAgent, action_HandObject: ERROR trajectoryrobot2d->go "<< ex << std::endl;
throw ex;
}
}
string state;
try
{
state = trajectoryrobot2d_proxy->getState().state;
}
catch(const Ice::Exception &ex)
{
std::cout <<"navigationAgent, action_HandObject: trajectoryrobot2d->getState().state "<< ex << std::endl;
throw ex;
}
//state="IDLE";
std::cout<<state<<" haveTarget "<<haveTarget;
if (state=="IDLE" && haveTarget)
{
//std::cout<<"\ttrajectoryrobot2d_proxy->getState() "<<trajectoryrobot2d_proxy->getState().state<<"\n";
try
{
// AGMModel::SPtr newModel(new AGMModel(worldModel));
// int statusID =symbols["status"]->identifier;
// newModel->getEdgeByIdentifiers(objectID, statusID, "noReach").setLabel("reach");
// sendModificationProposal(worldModel, newModel);
haveTarget=false;
}
catch (...)
{
std::cout<<"\neeeee"<< "\n";
}
}
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
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;
}
/**
* \brief Called when the robot is sent close to a person to offer the object
*/
void SpecificWorker::action_HandObject_Offer(bool newAction)
{
// Get symbols' map
std::map<std::string, AGMModelSymbol::SPtr> symbols;
try
{
symbols = worldModel->getSymbolsMap(params); //ALL THE SYMBOLS GIVEN IN THE RULE
}
catch(...)
{
printf("navigationAgent: Couldn't retrieve action's parameters\n");
printf("<<WORLD\n");
AGMModelPrinter::printWorld(worldModel);
printf("WORLD>>\n");
if (worldModel->size() > 0) { exit(-1); }
}
// Get target
int roomID, personID, robotID;
try
{
if (symbols["room"].get() and symbols["person"].get() and symbols["robot"].get() )
{
roomID = symbols["room"]->identifier;
personID =symbols["person"]->identifier;
robotID = symbols["robot"]->identifier;
try // If we can access the 'reach' edge for the object of the action
{ // is not really necessary. The planner is probably replanning.
worldModel->getEdgeByIdentifiers(personID, personID, "reach");
{
static QTime lastMsg = QTime::currentTime().addSecs(-1000);
if (lastMsg.elapsed() > 1000)
{
rDebug2(("navigationAgent ignoring action setHandObject_Offer (person currently reached)"));
lastMsg = QTime::currentTime();
return;
}
printf("ask the platform to stop\n");
stop();
}
}
catch(...)
{
}
}
else
{
printf("parameters not in the model yet\n");
return;
}
}
catch(...)
{
printf("ERROR: SYMBOL DOESN'T EXIST \n");
exit(2);
}
// GET THE INNERMODEL NAMES OF TH SYMBOLS
QString robotIMID;
QString roomIMID;
QString personIMID;
try
{
robotIMID = QString::fromStdString(worldModel->getSymbol(robotID)->getAttribute("imName"));
roomIMID = QString::fromStdString(worldModel->getSymbol(roomID)->getAttribute("imName"));
personIMID = QString::fromStdString(worldModel->getSymbol(personID)->getAttribute("imName"));
}
catch(...)
{
printf("ERROR IN GET THE INNERMODEL NAMES\n");
exit(2);
}
// GET THE TARGET POSE:
RoboCompTrajectoryRobot2D::TargetPose tgt;
try
{
if (not (innerModel->getNode(roomIMID) and innerModel->getNode(personIMID))) return;
QVec poseInRoom = innerModel->transform6D(roomIMID, personIMID); // FROM PERSON TO ROOM
qDebug() << __FUNCTION__ <<" Target pose: "<< poseInRoom;
tgt.x = poseInRoom.x();
tgt.y = 0;
tgt.z = poseInRoom.z();
tgt.rx = 0;
tgt.ry = poseInRoom.ry();
tgt.rz = 0;
tgt.doRotation = true;
}
catch (...)
{
qDebug()<< __FUNCTION__ << "InnerModel Exception. Element not found in tree";
}
// Execute target
try
{
try
{
QVec O = innerModel->transform("shellyArm_grasp_pose", personIMID);
QVec graspRef = innerModel->transform("robot", "shellyArm_grasp_pose");
go(tgt.x, tgt.z, 0, tgt.doRotation, graspRef.x(), graspRef.z(), 20);
qDebug() << __FUNCTION__ << "trajectoryrobot2d->go(" << tgt.x << ", " << tgt.z << ", " << tgt.ry << ", " << graspRef.x() << ", " << graspRef.z() << " )\n";
haveTarget = true;
}
catch(const RoboCompTrajectoryRobot2D::RoboCompException &ex)
{
std::cout << ex << " " << ex.text << std::endl;
throw;
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
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;
}
/**
* \brief Called when the robot is sent close to an object's location
*/
void SpecificWorker::action_SetObjectReach(bool newAction)
{ // Get symbols' map
std::map<std::string, AGMModelSymbol::SPtr> symbols;
try
{
symbols = worldModel->getSymbolsMap(params); //ALL THE SYMBOLS GIVEN IN THE RULE
}
catch(...)
{
printf("navigationAgent: Couldn't retrieve action's parameters\n");
printf("<<WORLD\n");
AGMModelPrinter::printWorld(worldModel);
printf("WORLD>>\n");
if (worldModel->size() > 0) { exit(-1); }
}
// Get target
int roomID, objectID, robotID;
try
{
if (symbols["room"].get() and symbols["object"].get() and symbols["robot"].get() )
{
roomID = symbols["room"]->identifier;
objectID =symbols["object"]->identifier;
robotID = symbols["robot"]->identifier;
try // If we can access the 'reach' edge for the object of the action
{ // is not really necessary. The planner is probably replanning.
worldModel->getEdgeByIdentifiers(objectID, objectID, "reach");
{
static QTime lastMsg = QTime::currentTime().addSecs(-1000);
if (lastMsg.elapsed() > 1000)
{
rDebug2(("navigationAgent ignoring action setObjectReach (object currently reached)"));
lastMsg = QTime::currentTime();
return;
}
printf("ask the platform to stop\n");
stop();
}
}
catch(...)
{
}
}
else
{
printf("parameters not in the model yet\n");
return;
}
}
catch(...)
{
printf("ERROR: SYMBOL DOESN'T EXIST \n");
exit(2);
}
// GET THE INNERMODEL NAMES OF TH SYMBOLS
QString robotIMID;
QString roomIMID;
QString objectIMID;
try
{
robotIMID = QString::fromStdString(worldModel->getSymbol(robotID)->getAttribute("imName"));
roomIMID = QString::fromStdString(worldModel->getSymbol(roomID)->getAttribute("imName"));
objectIMID = QString::fromStdString(worldModel->getSymbol(objectID)->getAttribute("imName"));
// check if object has reachPosition
AGMModelSymbol::SPtr object = worldModel->getSymbol(objectID);
for (auto edge = object->edgesBegin(worldModel); edge != object->edgesEnd(worldModel); edge++)
{
if (edge->getLabel() == "reachPosition")
{
const std::pair<int32_t, int32_t> symbolPair = edge->getSymbolPair();
objectID = symbolPair.second;
objectIMID = QString::fromStdString(worldModel->getSymbol(objectID)->getAttribute("imName"));
qDebug() << __FUNCTION__ << "Target object " << symbolPair.first<<"->"<<symbolPair.second<<" object "<<objectIMID;
}
}
}
catch(...)
{
printf("ERROR IN GET THE INNERMODEL NAMES\n");
exit(2);
}
// GET THE TARGET POSE:
RoboCompTrajectoryRobot2D::TargetPose tgt;
try
{
if (not (innerModel->getNode(roomIMID) and innerModel->getNode(objectIMID)))
{
printf("Cant find objects to reach %d\n", __LINE__);
return;
}
QVec poseInRoom = innerModel->transform6D(roomIMID, objectIMID); // FROM OBJECT TO ROOM
qDebug() << __FUNCTION__ <<" Target pose: "<< poseInRoom;
tgt.x = poseInRoom.x();
tgt.y = 0;
tgt.z = poseInRoom.z();
tgt.rx = 0;
//tgt.ry = 0;
tgt.ry = poseInRoom.ry(); //needed to reach tables
tgt.rz = 0;
tgt.doRotation = true;
}
catch (...)
{
qDebug()<< __FUNCTION__ << "InnerModel Exception. Element not found in tree";
}
// Execute target
try
{
// if (!haveTarget)
{
try
{
QVec O = innerModel->transform("shellyArm_grasp_pose", objectIMID);
//O.print(" O pose relativa");
//qDebug() << __FUNCTION__ << "O norm:" << O.norm2();
QVec graspRef = innerModel->transform("robot", "shellyArm_grasp_pose");
graspRef.print("graspRef");
float th=20;
tgt.ry = -3.1415926535/2.;
go(tgt.x, tgt.z, tgt.ry, tgt.doRotation, graspRef.x(), graspRef.z(), th);
qDebug() << __FUNCTION__ << "trajectoryrobot2d->go(" << tgt.x << ", " << tgt.z << ", " << tgt.ry << ", " << graspRef.x() << ", " << graspRef.z() << " )\n";
haveTarget = true;
}
catch(const RoboCompTrajectoryRobot2D::RoboCompException &ex)
{
std::cout << ex << " " << ex.text << std::endl;
throw;
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
string state;
try
{
state = trajectoryrobot2d_proxy->getState().state;
}
catch(const Ice::Exception &ex)
{
std::cout <<"trajectoryrobot2d->getState().state "<< ex << std::endl;
throw ex;
}
//state="IDLE";
std::cout<<state<<" haveTarget "<<haveTarget;
if (state=="IDLE" && haveTarget)
{
//std::cout<<"\ttrajectoryrobot2d_proxy->getState() "<<trajectoryrobot2d_proxy->getState().state<<"\n";
try
{
// AGMModel::SPtr newModel(new AGMModel(worldModel));
// int statusID =symbols["status"]->identifier;
// newModel->getEdgeByIdentifiers(objectID, statusID, "noReach").setLabel("reach");
// sendModificationProposal(worldModel, newModel);
haveTarget=false;
}
catch (...)
{
std::cout<<"\neeeee"<< "\n";
}
}
}
catch(const Ice::Exception &ex)
{
std::cout << ex << std::endl;
}
}
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;
}
void SpecificWorker::crearSubTarget()
{
qDebug() << __FUNCTION__ << "creando subTarget";
float dt;
QVec t = inner->transform ( "rgbd", ctarget.target, "world" );
float d = t.norm2();
float alpha =atan2 ( t.x(), t.z() );
uint i,j;
//const int R =400;
for ( i = 5; i<ldata.size()-5; i++ ) {
if ( ldata[i].angle < alpha ) {
if ( d>ldata[i].dist ) {
dt=ldata[i].dist;
break;
}
}
}
qDebug() << __FUNCTION__<<i;
qDebug() << __FUNCTION__<<ldata[i].dist<<ldata[i].angle;
for ( j = i; j<ldata.size()-5; j++ ) {
qDebug() << __FUNCTION__<<dt<< dt+ ( dt*0.2 ) <<ldata[j].dist << ldata[j].angle;
if ( ldata[j].dist> ( dt+ ( dt*0.2 ) ) and ldata[j].angle < 0 ) {
ctarget.subTarget=inner->transform ( "world", QVec::vec3 ( ldata[j].dist *sin ( ldata[j].angle )-2000,0, ldata[j].dist *cos ( ldata[j].angle ) ), "laser" );
ctarget.activeSub = true;
break;
}
}
qDebug() << __FUNCTION__<< "Subtargeet" << QVec::vec3 ( ldata[j].dist *sin ( ldata[j].angle ),0, ldata[j].dist *cos ( ldata[j].angle ) );
/*
//Search the first increasing step from the center to the right
uint i,j;
const int R =600;
for(i=ldata.size()/2; i>5; i--)
{
if( (ldata[i].dist - ldata[i-1].dist) < -R )
{
if(i<=7)
{
i=0;
break;
}
uint k=i-2;
while( (k >= 0) and (fabs( ldata[k].dist*sin(ldata[k].angle - ldata[i-1].angle)) < R ))
{ k--; }
i=k;
break;
}
}
for(j=ldata.size()/2-5; j<ldata.size()-1; j++)
{
if( (ldata[j].dist - ldata[j+1].dist) < -R )
{
if(j>ldata.size()-3)
{
j=ldata.size()-1;
break;
}
uint k=j+2;
while( (k < ldata.size()) and (fabs( ldata[k].dist*sin(ldata[k].angle - ldata[j+1].angle)) < R ))
{ k++; }
j=k;
break;
}
}
QVec sI = inner->transform("world", QVec::vec3(ldata[j].dist *sin(ldata[j].angle),0, ldata[j].dist *cos(ldata[j].angle)), "laser");
QVec sD = inner->transform("world", QVec::vec3(ldata[i].dist *sin(ldata[i].angle),0, ldata[i].dist *cos(ldata[i].angle)), "laser");
if( (sI-ctarget.target).norm2() > (sD-ctarget.target).norm2() )
ctarget.subtarget=sD;
else
ctarget.subtarget=sI;
ctarget.activeSub=true;
// for(i=5; i<ldata.size()-5;i++)
// {
// if(ldata[i-1].dist - ldata[i].dist > 400)
// {
// ctarget.subtarget=inner->transform("world", QVec::vec3(ldata[i].dist *sin(ldata[i].angle),0, ldata[i].dist *cos(ldata[i].angle)), "laser");
// ctarget.activeSub=true;
// break;
// }
//
// if(ldata[i+1].dist - ldata[i].dist > 400)
// {
//
// ctarget.subtarget=inner->transform("world", QVec::vec3(ldata[i].dist *sin(ldata[i].angle),0, ldata[i].dist *cos(ldata[i].angle)), "laser");
// ctarget.activeSub=true;
// break;
// }
//
// }
qDebug()<<ctarget.subtarget;
// qDebug()<<"distancia subtarget"<<ldata[i].dist;
// differentialrobot_proxy->setSpeedBase(0,0.5);
//exit(-1);*/
}
/**
* @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;
}