giving action is working in simulation

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: "<<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();
@@ -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: "<<px1.toString(3,3);
  }
- }
+ }*/
  break;
  }
  case 6 : {
@@ -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)
@@ -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;
  }
  }
@@ -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]; 
 

src/extras/configure.cpp

@@ -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;
@@ -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;
  }
 

src/icub_leader.cpp

@@ -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];
 
@@ -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;
  }