Skip to content

Commit

Permalink
giving action is working in simulation
Browse files Browse the repository at this point in the history
  • Loading branch information
@NunoDuarte
NunoDuarte committed May 24, 2018
1 parent 3898447 commit 358b557
Show file tree
Hide file tree
Showing 3 changed files with 68 additions and 36 deletions.
85 changes: 60 additions & 25 deletions src/extras/compute.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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)
Expand All @@ -341,28 +355,20 @@ using namespace yarp::math;
igaze->lookAtMonoPixel(0,px2); // 0: left image, 1: for right
yInfo()<<"gazing at Brick: "<<px2.toString(3,3);
}
}
}*/
break;
}
case 2 : {
cout << '2' << endl;

// look up if you haven't already
Vector ang(3,0.0);
igaze->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();
Expand All @@ -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();
Expand Down Expand Up @@ -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)
Expand All @@ -459,7 +480,7 @@ using namespace yarp::math;
igaze->lookAtMonoPixel(0,px1); // 0: left image, 1: for right
yInfo()<<"gazing at Teammate's Tower: "<<px1.toString(3,3);
}
}
}*/
break;
}
case 6 : {
Expand All @@ -469,12 +490,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 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)
Expand All @@ -498,7 +533,7 @@ using namespace yarp::math;
igaze->lookAtMonoPixel(0,px3); // 0: left image, 1: for right
yInfo()<<"gazing at My Tower: "<<px3.toString(3,3);
}
}
}*/
break;
}
}
Expand Down Expand Up @@ -769,15 +804,15 @@ using namespace yarp::math;
}
iarm->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 {
//closing the hand
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];

Expand Down
13 changes: 5 additions & 8 deletions src/extras/configure.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -194,29 +194,26 @@ 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);
thumb.push_back(8);
for (int i=9; i<16; i++)
fingers.push_back(i);

getchar();

// first, look down
look_down();

// initialize grasping and releasing counter
grasp = false;
released = false;

getchar();
// Select Action
// giving - 0
// placing - 1
int num = 1;
int num = 0;

string Result;
std::ostringstream Result_string;
Expand All @@ -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 = ("<<action <<")";
}
else
action = -1;

*/
return true;
}

Expand Down
6 changes: 3 additions & 3 deletions src/icub_leader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -142,14 +142,14 @@ using namespace std;
}*/

// begin
if (count < 40){
if (count < 38){
look = Eyes[0][0];

fixate(look);
yInfo()<<"fixating at ("<< look <<")";

// duration of action
}else if (count > 40 and count < 1000){
}else if (count > 38 and count < 1000){

look = Eyes[count][0];

Expand Down Expand Up @@ -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;
}

Expand Down

0 comments on commit 358b557

Please sign in to comment.