change parameters to work with the real robot

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)

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,22 +70,22 @@ 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))
  {
  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);