From e27c42d55f82437230580aeba030346203dd42ca Mon Sep 17 00:00:00 2001 From: AlexAntn Date: Wed, 3 Feb 2021 17:56:27 +0100 Subject: [PATCH 1/8] added warning message to position control PID when the configuration is not consistent across joints --- src/libraries/icubmod/embObjMotionControl/eomcParser.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp index 90c371233b..0f4e04fd1f 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp @@ -85,6 +85,7 @@ bool Parser::parsePids(yarp::os::Searchable &config, PidInfo *ppids/*, PidInfo * // come specificato in _currentControlLaw if(!parseSelectedCurrentPid(config, lowLevPidisMandatory, cpids)) // OK return false; + // legge i pid di velocità per ciascun motore // come specificato in _speedControlLaw if(!parseSelectedSpeedPid(config, lowLevPidisMandatory, spids)) // OK From 52a8a08a5e078f7e83b1c54aa0d9f26b9586eef3 Mon Sep 17 00:00:00 2001 From: Alexandre Antunes Date: Thu, 4 Feb 2021 15:07:17 +0100 Subject: [PATCH 2/8] revert character change in a comment Co-authored-by: Ugo Pattacini --- src/libraries/icubmod/embObjMotionControl/eomcParser.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp index 0f4e04fd1f..4505f076d0 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp @@ -2364,3 +2364,4 @@ bool Parser::checkJointTypes(PidInfo *pids, const std::string &pid_type) return true; } + From 045d24f89ec61b3880dc585754d4859839b53290 Mon Sep 17 00:00:00 2001 From: AlexAntn Date: Tue, 23 Feb 2021 14:52:22 +0100 Subject: [PATCH 3/8] implemented dynamic cast to account for torque pid type --- src/libraries/icubmod/embObjMotionControl/eomcParser.cpp | 6 ++++++ src/libraries/icubmod/embObjMotionControl/eomcParser.h | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp index 4505f076d0..f0de474990 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp @@ -2332,6 +2332,12 @@ bool Parser::checkJointTypes(PidInfo *pids, const std::string &pid_type) { //Here i would check that all joints have same type units in order to create pid_type helper with correct factor. + //parse verify if pid type is torque or some other + if(pid_type == "TORQUE") + { + dynamic_cast(pids); + } + //get first joint with enabled pid_type int firstjoint = -1; for(int i=0; i<_njoints; i++) diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.h b/src/libraries/icubmod/embObjMotionControl/eomcParser.h index 4cd15349af..d8e1e8a5c9 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.h +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.h @@ -178,7 +178,7 @@ class PidInfo fbk_PidUnits = yarp::dev::PidFeedbackUnitsEnum::RAW_MACHINE_UNITS; out_PidUnits = yarp::dev::PidOutputUnitsEnum::RAW_MACHINE_UNITS; } - ~PidInfo() + virtual ~PidInfo() { //delete (usernamePidSelected); } From 4e9d0b2dbd2bb71ce90f0eb5b14d5a34ec9829dc Mon Sep 17 00:00:00 2001 From: AlexAntn Date: Tue, 23 Feb 2021 15:05:40 +0100 Subject: [PATCH 4/8] reassigning the pointer from type PidInfo to TrqPidInfo --- src/libraries/icubmod/embObjMotionControl/eomcParser.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp index f0de474990..8a229e8cd7 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp @@ -2335,7 +2335,7 @@ bool Parser::checkJointTypes(PidInfo *pids, const std::string &pid_type) //parse verify if pid type is torque or some other if(pid_type == "TORQUE") { - dynamic_cast(pids); + TrqPidInfo* pids = dynamic_cast(pids); } //get first joint with enabled pid_type From 50173e994b42d2799c7889b1e725bb0888a75eff Mon Sep 17 00:00:00 2001 From: AlexAntn Date: Tue, 23 Feb 2021 15:45:41 +0100 Subject: [PATCH 5/8] splitting the check between torque and others --- .../embObjMotionControl/eomcParser.cpp | 74 ++++++++++++++----- 1 file changed, 54 insertions(+), 20 deletions(-) diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp index 8a229e8cd7..6cd2a848b0 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp @@ -2335,38 +2335,72 @@ bool Parser::checkJointTypes(PidInfo *pids, const std::string &pid_type) //parse verify if pid type is torque or some other if(pid_type == "TORQUE") { + // since we are working with a torque pid, we first cast it as such TrqPidInfo* pids = dynamic_cast(pids); - } - //get first joint with enabled pid_type - int firstjoint = -1; - for(int i=0; i<_njoints; i++) - { - if(pids[i].enabled) + //get first joint with enabled pid_type + int firstjoint = -1; + for(int i=0; i<_njoints; i++) { - firstjoint = i; - break; + if(pids[i].enabled) + { + firstjoint = i; + break; + } } - } - if(firstjoint==-1) - { - // no joint has current enabed - return true; - } + if(firstjoint==-1) + { + // no joint has current enabed + return true; + } - for(int i=firstjoint+1; i<_njoints; i++) + for(int i=firstjoint+1; i<_njoints; i++) + { + if(pids[i].enabled) + { + if(pids[firstjoint].fbk_PidUnits != pids[i].fbk_PidUnits || + pids[firstjoint].out_PidUnits != pids[i].out_PidUnits) + { + yError() << "embObjMC BOARD " << _boardname << "all joints with " << pid_type << " enabled should have same controlunits type. Joint " << firstjoint << " differs from joint " << i; + return false; + } + } + } + } + else { - if(pids[i].enabled) + //get first joint with enabled pid_type + int firstjoint = -1; + for(int i=0; i<_njoints; i++) { - if(pids[firstjoint].fbk_PidUnits != pids[i].fbk_PidUnits || - pids[firstjoint].out_PidUnits != pids[i].out_PidUnits) + if(pids[i].enabled) { - yError() << "embObjMC BOARD " << _boardname << "all joints with " << pid_type << " enabled should have same controlunits type. Joint " << firstjoint << " differs from joint " << i; - return false; + firstjoint = i; + break; + } + } + + if(firstjoint==-1) + { + // no joint has current enabed + return true; + } + + for(int i=firstjoint+1; i<_njoints; i++) + { + if(pids[i].enabled) + { + if(pids[firstjoint].fbk_PidUnits != pids[i].fbk_PidUnits || + pids[firstjoint].out_PidUnits != pids[i].out_PidUnits) + { + yError() << "embObjMC BOARD " << _boardname << "all joints with " << pid_type << " enabled should have same controlunits type. Joint " << firstjoint << " differs from joint " << i; + return false; + } } } } + return true; } From 9949cc6d82e728ba2384a6c90b16d3d64d1fe420 Mon Sep 17 00:00:00 2001 From: AlexAntn Date: Wed, 24 Feb 2021 12:12:36 +0100 Subject: [PATCH 6/8] fixed the checkJointTypes function for the case of TORQUE control --- .../icubmod/embObjMotionControl/eomcParser.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp index 6cd2a848b0..fbcf388018 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp @@ -2336,13 +2336,14 @@ bool Parser::checkJointTypes(PidInfo *pids, const std::string &pid_type) if(pid_type == "TORQUE") { // since we are working with a torque pid, we first cast it as such - TrqPidInfo* pids = dynamic_cast(pids); + // this allows the loop to correctly point to the corresponding memory + TrqPidInfo* trq_pids = (TrqPidInfo*) pids; //get first joint with enabled pid_type int firstjoint = -1; for(int i=0; i<_njoints; i++) { - if(pids[i].enabled) + if(trq_pids[i].enabled) { firstjoint = i; break; @@ -2357,10 +2358,10 @@ bool Parser::checkJointTypes(PidInfo *pids, const std::string &pid_type) for(int i=firstjoint+1; i<_njoints; i++) { - if(pids[i].enabled) + if(trq_pids[i].enabled) { - if(pids[firstjoint].fbk_PidUnits != pids[i].fbk_PidUnits || - pids[firstjoint].out_PidUnits != pids[i].out_PidUnits) + if(trq_pids[firstjoint].fbk_PidUnits != trq_pids[i].fbk_PidUnits || + trq_pids[firstjoint].out_PidUnits != trq_pids[i].out_PidUnits) { yError() << "embObjMC BOARD " << _boardname << "all joints with " << pid_type << " enabled should have same controlunits type. Joint " << firstjoint << " differs from joint " << i; return false; From 287736e1f1e2ab2a9eef8d071f13626d24ace105 Mon Sep 17 00:00:00 2001 From: AlexAntn Date: Wed, 24 Feb 2021 12:54:22 +0100 Subject: [PATCH 7/8] destructor of PidInfo does not need to be virtual --- src/libraries/icubmod/embObjMotionControl/eomcParser.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.h b/src/libraries/icubmod/embObjMotionControl/eomcParser.h index d8e1e8a5c9..4cd15349af 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.h +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.h @@ -178,7 +178,7 @@ class PidInfo fbk_PidUnits = yarp::dev::PidFeedbackUnitsEnum::RAW_MACHINE_UNITS; out_PidUnits = yarp::dev::PidOutputUnitsEnum::RAW_MACHINE_UNITS; } - virtual ~PidInfo() + ~PidInfo() { //delete (usernamePidSelected); } From 7473452be68c00db29ca302eb7ed8b0d845735dd Mon Sep 17 00:00:00 2001 From: AlexAntn Date: Wed, 24 Feb 2021 14:16:57 +0100 Subject: [PATCH 8/8] cleaning up comments and code --- .../embObjMotionControl/eomcParser.cpp | 77 ++++++++----------- .../icubmod/embObjMotionControl/eomcParser.h | 1 + 2 files changed, 34 insertions(+), 44 deletions(-) diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp index fbcf388018..6d1289e8c5 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp @@ -2330,79 +2330,68 @@ void JointsSet::dumpdata(void) bool Parser::checkJointTypes(PidInfo *pids, const std::string &pid_type) { - //Here i would check that all joints have same type units in order to create pid_type helper with correct factor. + //Here we check that all joints have same type units in order to create pid_type helper with correct factor. - //parse verify if pid type is torque or some other + int firstjoint = -1; + + // verify if pid type is torque or some other if(pid_type == "TORQUE") { // since we are working with a torque pid, we first cast it as such // this allows the loop to correctly point to the corresponding memory TrqPidInfo* trq_pids = (TrqPidInfo*) pids; - //get first joint with enabled pid_type - int firstjoint = -1; for(int i=0; i<_njoints; i++) { - if(trq_pids[i].enabled) + // if we already had an enabled PID, compare with current one + if(firstjoint != -1 && !checkSinglePid(trq_pids[firstjoint], trq_pids[i], firstjoint, i, pid_type)) { - firstjoint = i; - break; + return false; } - } - - if(firstjoint==-1) - { - // no joint has current enabed - return true; - } - - for(int i=firstjoint+1; i<_njoints; i++) - { - if(trq_pids[i].enabled) + // if we haven't found an enabled PID yet, and this one is enabled, save it + if(firstjoint == -1 && trq_pids[i].enabled) { - if(trq_pids[firstjoint].fbk_PidUnits != trq_pids[i].fbk_PidUnits || - trq_pids[firstjoint].out_PidUnits != trq_pids[i].out_PidUnits) - { - yError() << "embObjMC BOARD " << _boardname << "all joints with " << pid_type << " enabled should have same controlunits type. Joint " << firstjoint << " differs from joint " << i; - return false; - } + firstjoint = i; } } } else { - //get first joint with enabled pid_type - int firstjoint = -1; for(int i=0; i<_njoints; i++) { - if(pids[i].enabled) + // if we already had an enabled PID, compare with current one + if(firstjoint != -1 && !checkSinglePid(pids[firstjoint], pids[i], firstjoint, i, pid_type)) + { + return false; + } + // if we haven't found an enabled PID yet, and this one is enabled, save it + if(firstjoint == -1 && pids[i].enabled) { firstjoint = i; - break; } } + } - if(firstjoint==-1) - { - // no joint has current enabed - return true; - } + return true; +} - for(int i=firstjoint+1; i<_njoints; i++) +bool Parser::checkSinglePid(PidInfo &firstPid, PidInfo ¤tPid, const int &firstjoint, const int ¤tjoint, const std::string &pid_type) +{ + // check if the PID we are checking is enabled + if(currentPid.enabled) + { + // if it has different unit types from the previous enabled PIDs + if(firstPid.fbk_PidUnits != currentPid.fbk_PidUnits || + firstPid.out_PidUnits != currentPid.out_PidUnits) { - if(pids[i].enabled) - { - if(pids[firstjoint].fbk_PidUnits != pids[i].fbk_PidUnits || - pids[firstjoint].out_PidUnits != pids[i].out_PidUnits) - { - yError() << "embObjMC BOARD " << _boardname << "all joints with " << pid_type << " enabled should have same controlunits type. Joint " << firstjoint << " differs from joint " << i; - return false; - } - } + yError() << "embObjMC BOARD " << _boardname << "all joints with " << pid_type << " enabled should have same controlunits type. Joint " << firstjoint << " differs from joint " << currentjoint; + return false; } } - return true; } + + + diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.h b/src/libraries/icubmod/embObjMotionControl/eomcParser.h index 4cd15349af..e148ecbe79 100644 --- a/src/libraries/icubmod/embObjMotionControl/eomcParser.h +++ b/src/libraries/icubmod/embObjMotionControl/eomcParser.h @@ -371,6 +371,7 @@ class Parser bool parsePidUnitsType(yarp::os::Bottle &bPid, yarp::dev::PidFeedbackUnitsEnum &fbk_pidunits, yarp::dev::PidOutputUnitsEnum& out_pidunits); bool checkJointTypes(PidInfo *pids, const std::string &pid_type); + bool checkSinglePid(PidInfo &firstPid, PidInfo ¤tPid, const int &firstjoint, const int ¤tjoint, const std::string &pid_type); bool convert(std::string const &fromstring, eOmc_jsetconstraint_t &jsetconstraint, bool& formaterror); bool convert(yarp::os::Bottle &bottle, std::vector &matrix, bool &formaterror, int targetsize);