Improve application to acquire fingers data

src/tools/ergocub-fingers-tuning/README.md

@@ -27,7 +27,7 @@ The executable to run can be then found in the `build` folder.
 After building it, navigate to the `build` folder, and then launch it with your desired arguments, e.g.:
 
 ```bash
-./ident-finger --robot ergocub --part left_arm --joint-id 10 --cycles 5 --threshold 20 <degrees> --timeout 5000 <sec> --filename output.csv
+./ident-finger --port /ergocub/left_arm --joint-id 10 --cycles 5 --limits "(5 20)" <degrees> --pwm-values "(10 20 30)" <perc> --timeout 5 <sec> --filename output.csv
 ```
 
 #### move-finger-pos
@@ -47,10 +47,7 @@ Copy the `.csv` datasets created with `ident-finger` in the `data` folder. Then:
 1. Add to the path the folders `data` and `scripts`
 2. Open the live script `identification_tuning.mlx`
 3. Check the file names of the training and validation sets
-   - the file opened is the last one, unless the user run **init_ws** which resets the file name
-   - **init_ws** must also be run as the first command to insure the livescript works.
    - it's better to also 'clear all output' by right-clicking in the livescript otherwise the report can be filled with old data and plots!
 4. Run it by pressing "Run" or F5
    - sometimes (very often) the final command 'export' doesn't produce a complete pdf file. re-run this line by selecting it until the pdf looks fine 
 5. check the plots: if the fingers are not moving, then the PID gains are most likely not up to the task; e.g., see [here](https://github.com/icub-tech-iit/ergocub-design-lowerarm/issues/245#issuecomment-2095469706).
-

src/tools/ergocub-fingers-tuning/cpp/ident-finger/main.cpp

@@ -20,9 +20,9 @@
 #include <yarp/os/Bottle.h>
 #include <yarp/os/LogStream.h>
 #include <yarp/os/Network.h>
+#include <yarp/os/PeriodicThread.h>
 #include <yarp/os/ResourceFinder.h>
 #include <yarp/os/Time.h>
-#include <yarp/os/Value.h>
 
 using namespace yarp::os;
 using namespace yarp::dev;
@@ -35,135 +35,182 @@ struct DataExperiment {
   double pid_out;
 };
 
+IPositionControl *iPos{nullptr};
+IPWMControl *iPwm{nullptr};
+IControlMode *iCm{nullptr};
+IEncoders *iEnc{nullptr};
+IPidControl *iPid{nullptr};
+
+std::vector<DataExperiment> data_vec;
+
+class PWMThread : public PeriodicThread {
+private:
+  ResourceFinder rf_;
+  bool done_h = false;
+  bool done_l = false;
+  DataExperiment data;
+  uint8_t j = 0;
+  double pwm_ = 0;
+  double thr_min = 0;
+  double thr_max = 0;
+  double t0 = 0;
+
+public:
+  PWMThread(double ts, ResourceFinder &rf, uint8_t joint_id, double min,
+            double max)
+      : PeriodicThread(ts), rf_(rf), j(joint_id), thr_min(min), thr_max(max) {}
+
+  bool threadInit() {
+    t0 = Time::now();
+    return true;
+  }
+
+  void run() {
+    data.t = Time::now() - t0;
+    data.pwm_set = pwm_;
+    iEnc->getEncoder(j, &data.enc);
+    iPid->getPidOutput(VOCAB_PIDTYPE_POSITION, j, &data.pid_out);
+    iPwm->getDutyCycle(j, &data.pwm_read);
+
+    data_vec.push_back(std::move(data));
+
+    const bool above_max = data.enc >= thr_max && pwm_ < -.1;
+    const bool below_min = data.enc <= thr_min && pwm_ > .1;
+
+    if (above_max || below_min) {
+      if (above_max) {
+        done_h = true;
+      } else {
+        done_l = true;
+      }
+      pwm_ = -pwm_;
+      iPwm->setRefDutyCycle(j, pwm_);
+    }
+    yDebugThrottle(15, "Acquisition in progress...");
+  }
+
+  bool isDone() { return done_h && done_l; }
+
+  void setInputs(double pwm) {
+    pwm_ = pwm;
+    iPwm->setRefDutyCycle(j, pwm_);
+  }
+
+  void resetFlags() {
+    done_h = false;
+    done_l = false;
+  }
+};
+
+bool resetPosition(uint8_t j) {
+  iPwm->setRefDutyCycle(j, 0.0);
+  iCm->setControlMode(j, VOCAB_CM_POSITION);
+  iPos->setRefSpeed(j, 25.0);
+  iPos->setRefAcceleration(j, std::numeric_limits<double>::max());
+  iPos->positionMove(j, 0.);
+  bool done = false;
+
+  while (!done) {
+    iPos->checkMotionDone(j, &done);
+    Time::yield();
+  }
+
+  return done;
+}
+
 int main(int argc, char *argv[]) {
   Network yarp;
+
   if (!yarp.checkNetwork()) {
     yError() << "Unable to find YARP server!";
     return EXIT_FAILURE;
   }
 
   ResourceFinder rf;
   rf.configure(argc, argv);
+  rf.setVerbose(false);
+  rf.setQuiet(true);
 
   auto port = rf.check("port", Value("/ergocub/left_arm")).asString();
-  auto joint_id = rf.check("joint-id", Value(1)).asInt8();
-  auto cycles = rf.check("cycles", Value(10)).asInt32();
-  auto threshold = rf.check("threshold", Value(20)).asInt32();
-  auto timeout = rf.check("timeout", Value(5000)).asInt32();
+  auto cycles = rf.check("cycles", Value(2)).asInt32();
+  auto timeout = rf.check("timeout", Value(5.0)).asFloat64();
   auto filename = rf.check("filename", Value("output.csv")).asString();
-  auto pwm_list = rf.find("pwm_values").asList();
+  auto pwm_list = rf.find("pwm-values").asList();
+  auto Ts = rf.check("period", Value(0.001)).asFloat64();
+  auto joint_id = rf.check("joint-id", Value(12)).asInt8();
+  auto limits = rf.find("limits").asList();
 
-  PolyDriver m_driver;
-  IPositionControl *iPos{nullptr};
-  IPWMControl *iPwm{nullptr};
-  IControlMode *iCm{nullptr};
-  IEncoders *iEnc{nullptr};
-  IPidControl *iPid{nullptr};
+  if (limits->size() <= 1) {
+    yError("Insufficient number of limits specified, exiting.");
+    return EXIT_FAILURE;
+  }
+  auto thr_min = limits->get(0).asFloat64();
+  auto thr_max = limits->get(1).asFloat64();
+
+  if (pwm_list->size() <= 0) {
+    yError("No PWM values were specified, exiting.");
+    return EXIT_FAILURE;
+  }
 
   Property conf;
   conf.put("device", "remote_controlboard");
   conf.put("remote", port);
-  conf.put("local", "/logger");
+  conf.put("local", "/ident");
+  conf.put("carrier", "fast_tcp");
 
+  PolyDriver m_driver;
   if (!m_driver.open(conf)) {
-    yError() << "Failed to open";
+    yError() << "Failed to open" << port;
     return EXIT_FAILURE;
   }
 
   if (!(m_driver.view(iPos) && m_driver.view(iPwm) && m_driver.view(iCm) &&
         m_driver.view(iEnc) && m_driver.view(iPid))) {
     m_driver.close();
-    yError() << "Failed to view interfaces";
+    yError("Failed to view interfaces");
     return EXIT_FAILURE;
   }
 
-  std::vector<double> pwm_values; //{10, 25, 50, 75};
-
-  for (size_t i = 0; i < pwm_list->size(); ++i) {
-    pwm_values.push_back(pwm_list->get(i).asFloat64());
+  if (int N = 0; iPos->getAxes(&N), (joint_id >= N)) {
+    yError("Invalid joint-id");
+    return EXIT_FAILURE;
   }
 
-  std::ofstream file;
+  Time::delay(0.05); // add small delay before reading values
 
+  std::unique_ptr<PWMThread> pwm_thread =
+      std::make_unique<PWMThread>(Ts, rf, joint_id, thr_min, thr_max);
+
+  std::ofstream file;
   file.open(filename);
+  data_vec.reserve(10000);
 
-  std::vector<DataExperiment> data_vec;
-  data_vec.reserve(100000);
+  resetPosition(joint_id);
+  iCm->setControlMode(joint_id, VOCAB_CM_PWM);
+  iPwm->setRefDutyCycle(joint_id, 0.0);
 
-  // set control mode to position
-  iCm->setControlMode(joint_id, VOCAB_CM_POSITION);
-  // Homing
-  iPos->setRefSpeed(joint_id, 5.);
-  iPos->setRefAcceleration(joint_id, std::numeric_limits<double>::max());
-  iPos->positionMove(joint_id, 0.);
-  auto done{false};
-  while (!done) {
-    iPos->checkMotionDone(joint_id, &done);
-    Time::yield();
-  }
+  pwm_thread->start();
 
-  DataExperiment data;
-  auto t0 = Time::now();
-  auto t1{t0};
+  yInfo("Started data collection: %lu pwm values, %d cycles", pwm_list->size(),
+        cycles);
 
-  // set control mode to pwm
-  iCm->setControlMode(joint_id, VOCAB_CM_PWM);
-  iPwm->setRefDutyCycle(joint_id, 0.0);
+  for (int j = 0; j < pwm_list->size(); j++) {
 
-  // drives the joint between +20/-20 degrees for each vlaue of PWM stored in
-  // pwm_values
-  for (auto pwm : pwm_values) {
-    uint t = 0;
-    for (int i = 0; i < cycles * 2; i++) {
-      auto done_h = false;
-      auto done_l = false;
-      done = false;
-      pwm = -pwm;
-      iPwm->setRefDutyCycle(joint_id, pwm);
-
-      while ((!done_h || !done_l) && t < timeout) {
-        data.t = Time::now() - t0;
-        data.pwm_set = pwm;
-        iEnc->getEncoder(joint_id, &data.enc);
-        iPid->getPidOutput(VOCAB_PIDTYPE_POSITION, joint_id, &data.pid_out);
-        iPwm->getDutyCycle(joint_id, &data.pwm_read);
-
-        // if the joint exceeds +20/-20 degrees invert the PWM
-        if (Time::now() - t1 >= .001) {
-          //       yDebug() << data.pwm << " " << data.enc;
-          t++;
-          if (data.enc > threshold) {
-            if (!done_h) {
-              pwm = -pwm;
-              iPwm->setRefDutyCycle(joint_id, pwm);
-              done_h = true;
-            }
-          } else if (data.enc < threshold * -1) {
-            if (!done_l) {
-              pwm = -pwm;
-              iPwm->setRefDutyCycle(joint_id, pwm);
-              done_l = true;
-            }
-          }
-          data_vec.push_back(std::move(data));
-          t1 = Time::now();
-        }
+    pwm_thread->setInputs(pwm_list->get(j).asFloat64());
+
+    for (int i = 0; i < cycles; i++) {
+      while (!pwm_thread->isDone() &&
+             pwm_thread->getIterations() < (timeout / Ts)) {
+        Time::delay(0.05);
       }
+      pwm_thread->resetFlags();
+      pwm_thread->resetStat();
     }
   }
-  // sets PWM to zero
-  iPwm->setRefDutyCycle(joint_id, 0.0);
-  // sets control mode to position
-  iCm->setControlMode(joint_id, VOCAB_CM_POSITION);
-  // Homing
-  iPos->setRefSpeed(joint_id, 5.);
-  iPos->setRefAcceleration(joint_id, std::numeric_limits<double>::max());
-  iPos->positionMove(joint_id, 0.);
-  while (!done) {
-    iPos->checkMotionDone(joint_id, &done);
-    Time::yield();
-  }
-  // writes the log
+
+  pwm_thread->stop();
+  resetPosition(joint_id);
+
   for (const auto &d : data_vec) {
     file << d.t << "," << d.pwm_read << "," << d.enc << "," << d.pid_out << ","
          << d.pwm_set << std::endl;
@@ -172,6 +219,6 @@ int main(int argc, char *argv[]) {
   m_driver.close();
   file.close();
 
-  yInfo() << "Main returning...";
+  yInfo("Done");
   return EXIT_SUCCESS;
 }