diff --git a/src/tools/ergocub-fingers-tuning/README.md b/src/tools/ergocub-fingers-tuning/README.md index 8a6b9f9a..745a447f 100644 --- a/src/tools/ergocub-fingers-tuning/README.md +++ b/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 --timeout 5000 --filename output.csv +./ident-finger --port /ergocub/left_arm --joint-id 10 --cycles 5 --limits "(5 20)" --pwm-values "(10 20 30)" --timeout 5 --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). - diff --git a/src/tools/ergocub-fingers-tuning/cpp/ident-finger/main.cpp b/src/tools/ergocub-fingers-tuning/cpp/ident-finger/main.cpp index a0d37c39..75097eb9 100644 --- a/src/tools/ergocub-fingers-tuning/cpp/ident-finger/main.cpp +++ b/src/tools/ergocub-fingers-tuning/cpp/ident-finger/main.cpp @@ -20,9 +20,9 @@ #include #include #include +#include #include #include -#include using namespace yarp::os; using namespace yarp::dev; @@ -35,8 +35,92 @@ struct DataExperiment { double pid_out; }; +IPositionControl *iPos{nullptr}; +IPWMControl *iPwm{nullptr}; +IControlMode *iCm{nullptr}; +IEncoders *iEnc{nullptr}; +IPidControl *iPid{nullptr}; + +std::vector 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::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; @@ -44,126 +128,89 @@ int main(int argc, char *argv[]) { 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 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 pwm_thread = + std::make_unique(Ts, rf, joint_id, thr_min, thr_max); + + std::ofstream file; file.open(filename); + data_vec.reserve(10000); - std::vector 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::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::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; } diff --git a/src/tools/ergocub-fingers-tuning/identification_tuning.mlx b/src/tools/ergocub-fingers-tuning/identification_tuning.mlx index f79e9f24..0f8c8f9c 100644 Binary files a/src/tools/ergocub-fingers-tuning/identification_tuning.mlx and b/src/tools/ergocub-fingers-tuning/identification_tuning.mlx differ