From ca8463c4beeb2aa428b75f3c8cd4cabec5094101 Mon Sep 17 00:00:00 2001
From: NunoDuarte <username@users.noreply.github.com>
Date: Thu, 24 May 2018 14:41:02 +0100
Subject: [PATCH] change parameters to work with the real robot

---
 src/extras/compute.cpp   | 26 +++++++++++++++++++++-----
 src/extras/configure.cpp | 14 +++++++-------
 2 files changed, 28 insertions(+), 12 deletions(-)

diff --git a/src/extras/compute.cpp b/src/extras/compute.cpp
index cc5fcba..1c12bf2 100644
--- a/src/extras/compute.cpp
+++ b/src/extras/compute.cpp
@@ -28,13 +28,13 @@ using namespace yarp::math;
     {
         igaze->setSaccadesMode(true);  // this gives problem with waitMotionDone in simulation
         igaze->lookAtFixationPointSync(x);
-        double timeout = 10.0; 
+        /*double timeout = 10.0; 
         bool done = false; 
         done = igaze->waitMotionDone(0.1,timeout); 
         if(!done){
             yWarning("Something went wrong with the initial approach, using timeout");
             igaze->stopControl();
-        }
+        }*/
     }
 
     /***************************************************/
@@ -380,12 +380,12 @@ using namespace yarp::math;
                         cout << '3' << endl; 
 
                         // look up if you haven't already
-                        Vector ang(3,0.0);
+                        /*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
@@ -445,7 +445,7 @@ using namespace yarp::math;
                         yInfo()<<"fixating the Teammates Tower";
 
                         Vector look = x;
-                        look[0] = -0.35;
+                        look[0] = -0.45;
                         look[1] =  0.00;
                         look[2] = -0.05;         
 
@@ -753,6 +753,22 @@ using namespace yarp::math;
             yInfo()<<"Yielding new target: "<<target<<" [deg]";
             ipos->positionMove(j,target);
         }
+        // wait (with timeout) until the movement is completed
+        bool done=false;
+        double t0=Time::now();
+        while (!done && (Time::now()-t0 < 5.0))
+        {
+            yInfo()<<"Waiting...";
+            Time::delay(0.1);   // release the quantum to avoid starving resources
+            ipos->checkMotionDone(&done);
+        }
+
+        if (done)
+            yInfo()<<"Movement completed";
+        else
+            yWarning()<<"Timeout expired";
+
+        // wait until all fingers have attained their set-points
     }
 
     void ControlThread::reachArmGiving(Vector desired_p, Vector orientation, Vector x_pos, Vector velocity)
diff --git a/src/extras/configure.cpp b/src/extras/configure.cpp
index fc88c2f..7a7e9d2 100644
--- a/src/extras/configure.cpp
+++ b/src/extras/configure.cpp
@@ -22,7 +22,7 @@ using namespace yarp::math;
         printf("ControlThread:starting\n");
 
         // Open cartesian solver for right and left arm
-        string robot="icubSim";
+        string robot="icub";
 
         if (!openCartesian(robot,"left_arm"))
         {
@@ -60,7 +60,7 @@ using namespace yarp::math;
         // open a client interface to connect to the joint controller
         Property optJoint;
         optJoint.put("device","remote_controlboard");
-        optJoint.put("remote","/icubSim/left_arm");
+        optJoint.put("remote","/"+ robot +"/left_arm");
         optJoint.put("local","/position/left_arm");
 
         if (!drvHandL.open(optJoint))
@@ -70,9 +70,9 @@ using namespace yarp::math;
         }
 
         // open a client interface to connect to the joint controller
-        Property optJoint1;
+/*        Property optJoint1;
         optJoint1.put("device","remote_controlboard");
-        optJoint1.put("remote","/icubSim/right_arm");
+        optJoint1.put("remote","/"+ robot +"/right_arm");
         optJoint1.put("local","/position/right_arm");
 
         if (!drvHandR.open(optJoint1))
@@ -80,12 +80,12 @@ using namespace yarp::math;
             yError()<<"Unable to connect to /icubSim/right_arm";
             return false;
         }
-
+*/
         yInfo()<<"Begin moving arms to first initial position";
         // GET ARMS IN THE CORRECT POSITION
         x.resize(3);
-        x[1] =  0.5; // to the right
-        initArm(x);
+//        x[1] =  0.5; // to the right
+//        initArm(x);
         x[1] =  -0.5; // to the left
         initArm(x);