From 358b557323b374ee76d47c4315321c50debb3875 Mon Sep 17 00:00:00 2001 From: NunoDuarte Date: Thu, 24 May 2018 12:30:58 +0100 Subject: [PATCH] giving action is working in simulation --- src/extras/compute.cpp | 85 ++++++++++++++++++++++++++++------------ src/extras/configure.cpp | 13 +++--- src/icub_leader.cpp | 6 +-- 3 files changed, 68 insertions(+), 36 deletions(-) diff --git a/src/extras/compute.cpp b/src/extras/compute.cpp index 7d25fa9..cc5fcba 100644 --- a/src/extras/compute.cpp +++ b/src/extras/compute.cpp @@ -237,7 +237,7 @@ using namespace yarp::math; // without that, we cannot retrieve good 3D positions // with the real robot Vector ang(3,0.0); - ang[1]=-40.0; + ang[1]=-70.0; //ang[2]=-20; igaze->lookAtAbsAngles(ang); @@ -315,8 +315,22 @@ using namespace yarp::math; igaze->lookAtAbsAngles(ang); } + Vector x, o; + iarm->getPose(x,o); //get current position of hand + yInfo()<<"fixating the Brick"; + + Vector look = x; + look[0] = -0.20; + look[1] = -0.05; + look[2] = -0.02; + + igaze->lookAtFixationPoint(look); + //igaze->waitMotionDone(); + //to track from now on + igaze->setTrackingMode(true); + break; // look for red ball - Bottle *pTarget=port.read(false); + /*Bottle *pTarget=port.read(false); if (pTarget!=NULL) { if (pTarget->size()>2) @@ -341,28 +355,20 @@ using namespace yarp::math; igaze->lookAtMonoPixel(0,px2); // 0: left image, 1: for right yInfo()<<"gazing at Brick: "<getAngles(ang); - if (ang[1] > -30){ - ang[1]=-40.0; - igaze->lookAtAbsAngles(ang); - } - Vector x, o; iarm->getPose(x,o); //get current position of hand yInfo()<<"fixating the Human's face"; Vector look = x; - look[0] = -0.55; - look[1] = 0.00; - look[2] = 0.45; + look[0] = -0.35; + look[1] = -0.04; + look[2] = 0.40; igaze->lookAtFixationPoint(look); //igaze->waitMotionDone(); @@ -386,9 +392,9 @@ using namespace yarp::math; yInfo()<<"fixating the Human's hand"; Vector look = x; - look[0] = -0.55; - look[1] = 0.00; - look[2] = 0.25; + look[0] = -0.35; + look[1] = -0.04; + look[2] = 0.20; igaze->lookAtFixationPoint(look); //igaze->waitMotionDone(); @@ -430,11 +436,26 @@ using namespace yarp::math; Vector ang(3,0.0); igaze->getAngles(ang); if (ang[1] > -30){ - ang[1]=-40.0; + ang[1]=-70.0; igaze->lookAtAbsAngles(ang); } + + Vector x, o; + iarm->getPose(x,o); //get current position of hand + yInfo()<<"fixating the Teammates Tower"; + + Vector look = x; + look[0] = -0.35; + look[1] = 0.00; + look[2] = -0.05; + + igaze->lookAtFixationPoint(look); + //igaze->waitMotionDone(); + //to track from now on + igaze->setTrackingMode(true); + break; // look for red ball - Bottle *pTarget=port.read(false); + /*Bottle *pTarget=port.read(false); if (pTarget!=NULL) { if (pTarget->size()>2) @@ -459,7 +480,7 @@ using namespace yarp::math; igaze->lookAtMonoPixel(0,px1); // 0: left image, 1: for right yInfo()<<"gazing at Teammate's Tower: "<getAngles(ang); if (ang[1] > -30){ - ang[1]=-40.0; + ang[1]=-70.0; igaze->lookAtAbsAngles(ang); } + Vector x, o; + iarm->getPose(x,o); //get current position of hand + yInfo()<<"fixating My Tower"; + + Vector look = x; + look[0] = -0.20; + look[1] = 0.00; + look[2] = -0.05; + + igaze->lookAtFixationPoint(look); + //igaze->waitMotionDone(); + //to track from now on + igaze->setTrackingMode(true); + break; // look for red ball - Bottle *pTarget=port.read(false); + /*Bottle *pTarget=port.read(false); if (pTarget!=NULL) { if (pTarget->size()>2) @@ -498,7 +533,7 @@ using namespace yarp::math; igaze->lookAtMonoPixel(0,px3); // 0: left image, 1: for right yInfo()<<"gazing at My Tower: "<goToPose(x_pos,orientation); - if (count < 20 ){ +/* if (count < 20 ){ // let's put the hand in the pre-grasp configuration moveFingers(_hand, fingers, 0.0); } else { @@ -777,7 +812,7 @@ using namespace yarp::math; moveFingers(_hand, thumb, 1.0); moveFingers(_hand, fingers, 0.5); } - +*/ yInfo() << "v[0]:" << vcur[0] << "v[1]" << vcur[1] << "v[2]" << vcur[2]; yInfo() << "w[0]:" << wcur[0] << "w[1]" << wcur[1] << "w[2]" << wcur[2]; diff --git a/src/extras/configure.cpp b/src/extras/configure.cpp index ed44422..fc88c2f 100644 --- a/src/extras/configure.cpp +++ b/src/extras/configure.cpp @@ -194,8 +194,8 @@ using namespace yarp::math; xf[2] = 0.15; // get arm in initial position - x[1] = -0.5; // to the left - startingArm(x); + //x[1] = -0.5; // to the left + //startingArm(x); // define the fingers joints abduction.push_back(7); @@ -203,8 +203,6 @@ using namespace yarp::math; for (int i=9; i<16; i++) fingers.push_back(i); - getchar(); - // first, look down look_down(); @@ -212,11 +210,10 @@ using namespace yarp::math; grasp = false; released = false; - getchar(); // Select Action // giving - 0 // placing - 1 - int num = 1; + int num = 0; string Result; std::ostringstream Result_string; @@ -226,13 +223,13 @@ using namespace yarp::math; Eyes = loadDataFile("gazeBehavior_" + Result + ".txt", convert = false); - if (act.getAction(action)) +/* if (act.getAction(action)) { yInfo()<<" retrieved Action = ("< 40 and count < 1000){ + }else if (count > 38 and count < 1000){ look = Eyes[count][0]; @@ -243,7 +243,7 @@ int main(int argc, char *argv[]) double startTime=Time::now(); while(!done) { - if ((Time::now()-startTime)>100) + if ((Time::now()-startTime)>50) done=true; }