From 13c8d3054956a0789e9c7eb6f4962b2ac3c71e6b Mon Sep 17 00:00:00 2001 From: Ugo Pattacini Date: Mon, 24 Oct 2022 23:05:33 +0200 Subject: [PATCH] [ikin] fix handling of limbs versions --- src/libraries/actionPrimitives/CMakeLists.txt | 2 +- .../actionPrimitives/src/actionPrimitives.cpp | 6 +- .../iKin/include/iCub/iKin/iKinFwd.h | 124 +++++++++- src/libraries/iKin/src/iKinFwd.cpp | 224 ++++++++++++++---- src/libraries/iKin/src/iKinSlv.cpp | 14 +- .../ClientCartesianController.cpp | 6 +- .../ServerCartesianController.cpp | 10 +- .../gazeController/ClientGazeController.cpp | 6 +- .../src/MotorThread.cpp | 4 +- .../cartesianInterfaceControl/CMakeLists.txt | 2 +- .../cartesianInterfaceControl/main.cpp | 9 +- src/modules/iKinGazeCtrl/include/iCub/utils.h | 4 +- src/modules/iKinGazeCtrl/src/controller.cpp | 8 +- src/modules/iKinGazeCtrl/src/localizer.cpp | 6 +- src/modules/iKinGazeCtrl/src/main.cpp | 40 ++-- src/modules/iKinGazeCtrl/src/solver.cpp | 16 +- src/modules/iKinGazeCtrl/src/utils.cpp | 14 +- 17 files changed, 362 insertions(+), 133 deletions(-) diff --git a/src/libraries/actionPrimitives/CMakeLists.txt b/src/libraries/actionPrimitives/CMakeLists.txt index 745277a9ea..7f4f26ef4d 100644 --- a/src/libraries/actionPrimitives/CMakeLists.txt +++ b/src/libraries/actionPrimitives/CMakeLists.txt @@ -13,7 +13,7 @@ add_library(ICUB::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) target_include_directories(${PROJECT_NAME} PUBLIC "$" "$/${CMAKE_INSTALL_INCLUDEDIR}>") -target_link_libraries(${PROJECT_NAME} perceptiveModels ${YARP_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} iKin perceptiveModels ${YARP_LIBRARIES}) set_target_properties(${PROJECT_NAME} PROPERTIES diff --git a/src/libraries/actionPrimitives/src/actionPrimitives.cpp b/src/libraries/actionPrimitives/src/actionPrimitives.cpp index 8dddd5d78d..0b08fabef2 100644 --- a/src/libraries/actionPrimitives/src/actionPrimitives.cpp +++ b/src/libraries/actionPrimitives/src/actionPrimitives.cpp @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -57,6 +58,7 @@ using namespace yarp::dev; using namespace yarp::sig; using namespace yarp::math; using namespace iCub::ctrl; +using namespace iCub::iKin; using namespace iCub::perception; using namespace iCub::action; @@ -407,9 +409,9 @@ bool ActionPrimitives::handleTorsoDOF(Property &opt, const string &key) { Bottle info; cartCtrl->getInfo(info); - double hwver=info.find("arm_version").asFloat64(); + iKinLimbVersion hwver(info.find("arm_version").asString()); map remap{{ACTIONPRIM_TORSO_PITCH,0},{ACTIONPRIM_TORSO_ROLL,1},{ACTIONPRIM_TORSO_YAW,2}}; - if (hwver>=3.0) + if (hwver>=iKinLimbVersion("3.0")) { remap[ACTIONPRIM_TORSO_ROLL]=0; remap[ACTIONPRIM_TORSO_PITCH]=1; diff --git a/src/libraries/iKin/include/iCub/iKin/iKinFwd.h b/src/libraries/iKin/include/iCub/iKin/iKinFwd.h index 7d0661f9f9..7c6028c09f 100644 --- a/src/libraries/iKin/include/iCub/iKin/iKinFwd.h +++ b/src/libraries/iKin/include/iCub/iKin/iKinFwd.h @@ -1037,6 +1037,118 @@ class iKinLimb : public iKinChain }; +/** +* \ingroup iKinFwd +* +* A class for defining the versions of the iCub limbs. +*/ +class iKinLimbVersion +{ +protected: + unsigned long int major; + unsigned long int minor; + +public: + /** + * Default Constructor. + */ + iKinLimbVersion(); + + /** + * Constructor. + * @param version is a string for specifying different + * kinematics, e.g., "1.23". + */ + iKinLimbVersion(const std::string &version); + + /** + * Constructor. + * @param major the major version. + * @param minor the mninor version. + */ + iKinLimbVersion(const unsigned long int major, const unsigned long int minor); + + /** + * Copy Constructor. + */ + iKinLimbVersion(const iKinLimbVersion& v); + + /** + * Return the major version. + * @return the major version. + */ + unsigned long int get_major() const; + + /** + * Return the minor version. + * @return the minor version. + */ + unsigned long int get_minor() const; + + /** + * Return the version string. + * @return the version string. + */ + std::string get_version() const; + + /** + * Overloaded assignment operator. + * @param v the version to be assigned. + * @return . + */ + iKinLimbVersion& operator=(const iKinLimbVersion& v); + + /** + * Overloaded < operator. + * @param v the version to compare with. + * @return true iff < v. + */ + bool operator<(const iKinLimbVersion& v) const; + + /** + * Overloaded <= operator. + * @param v the version to compare with. + * @return true iff <= v. + */ + bool operator<=(const iKinLimbVersion& v) const; + + /** + * Overloaded == operator. + * @param v the version to compare with. + * @return true iff == v. + */ + bool operator==(const iKinLimbVersion& v) const; + + /** + * Overloaded != operator. + * @param v the version to compare with. + * @return true iff != v. + */ + bool operator!=(const iKinLimbVersion& v) const; + + /** + * Overloaded >= operator. + * @param v the version to compare with. + * @return true iff >= v. + */ + bool operator>=(const iKinLimbVersion& v) const; + + /** + * Overloaded > operator. + * @param v the version to compare with. + * @return true iff > v. + */ + bool operator>(const iKinLimbVersion& v) const; + + /** + * Overloaded - operator. + * @param v the version to compare with. + * @return the distance from v. + */ + iKinLimbVersion operator-(const iKinLimbVersion& v) const; +}; + + /** * \ingroup iKinFwd * @@ -1045,7 +1157,7 @@ class iKinLimb : public iKinChain class iCubTorso : public iKinLimb { protected: - double version; + iKinLimbVersion version; virtual void allocate(const std::string &_type); @@ -1081,7 +1193,7 @@ class iCubTorso : public iKinLimb class iCubArm : public iKinLimb { protected: - double version; + iKinLimbVersion version; virtual void allocate(const std::string &_type); @@ -1236,7 +1348,7 @@ class iCubFinger : public iKinLimb class iCubLeg : public iKinLimb { protected: - double version; + iKinLimbVersion version; virtual void allocate(const std::string &_type); @@ -1273,7 +1385,7 @@ class iCubLeg : public iKinLimb class iCubEye : public iKinLimb { protected: - double version; + iKinLimbVersion version; virtual void allocate(const std::string &_type); @@ -1364,7 +1476,7 @@ class iCubHeadCenter : public iCubEye class iCubInertialSensor : public iKinLimb { protected: - double version; + iKinLimbVersion version; virtual void allocate(const std::string &_type); @@ -1401,7 +1513,7 @@ class iCubInertialSensor : public iKinLimb class iCubInertialSensorWaist : public iKinLimb { protected: - double version; + iKinLimbVersion version; virtual void allocate(const std::string &_type); diff --git a/src/libraries/iKin/src/iKinFwd.cpp b/src/libraries/iKin/src/iKinFwd.cpp index 9253d2df0f..4c629343bc 100644 --- a/src/libraries/iKin/src/iKinFwd.cpp +++ b/src/libraries/iKin/src/iKinFwd.cpp @@ -25,15 +25,6 @@ using namespace yarp::math; using namespace iCub::ctrl; using namespace iCub::iKin; -namespace iCub { - namespace iKin { - constexpr double version_epsilon = 1e-6; - inline bool is_version(const double v1, const double v2) { - return (fabs(v1 - v2) < version_epsilon); - } - } -} - /************************************************************************/ void iCub::iKin::notImplemented(const unsigned int verbose) @@ -1550,6 +1541,134 @@ void iKinLimb::dispose() } +/************************************************************************/ +iKinLimbVersion::iKinLimbVersion() : major(0), minor(0) +{ +} + + +/************************************************************************/ +iKinLimbVersion::iKinLimbVersion(const string &version) +{ + size_t point=version.find('.'); + if (point!=string::npos) + { + major=strtoul(version.substr(1,point).c_str(),NULL,0); + minor=strtoul(version.substr(point+2).c_str(),NULL,0); + } + else + { + major=strtoul(version.substr(1).c_str(),NULL,0); + minor=0U; + } +} + + +/************************************************************************/ +iKinLimbVersion::iKinLimbVersion(const unsigned long int major, + const unsigned long int minor) +{ + this->major=major; + this->minor=minor; +} + + +/************************************************************************/ +iKinLimbVersion::iKinLimbVersion(const iKinLimbVersion& v) +{ + major=v.major; + minor=v.minor; +} + + +/************************************************************************/ +unsigned long int iKinLimbVersion::get_major() const +{ + return major; +} + + +/************************************************************************/ +unsigned long int iKinLimbVersion::get_minor() const +{ + return minor; +} + + +/************************************************************************/ +string iKinLimbVersion::get_version() const +{ + ostringstream version; + version<(const iKinLimbVersion& v) const +{ + return !(*this=(const iKinLimbVersion& v) const +{ + return (*this>v) || (*this==v); +} + + +/************************************************************************/ +iKinLimbVersion iKinLimbVersion::operator-(const iKinLimbVersion& v) const +{ + unsigned long int m1=(major>v.major)?major-v.major:v.major-major; + unsigned long int m2=(minor>v.minor)?minor-v.minor:v.minor-minor; + return iKinLimbVersion(m1,m2); +} + + /************************************************************************/ iCubTorso::iCubTorso() : iKinLimb(string("v1")) { @@ -1568,15 +1687,15 @@ iCubTorso::iCubTorso(const string &_type) : iKinLimb(_type) void iCubTorso::allocate(const string &_type) { if (getType().size()>1) - version=strtod(getType().substr(1).c_str(),NULL); + version=iKinLimbVersion(getType().substr(1)); else - version=1.0; + version=iKinLimbVersion("1.0"); Matrix H0(4,4); H0.zero(); H0(3,3)=1.0; - if (version<3.0) + if (version &lim) if (!limTorso.getLimits(iTorso,&min,&max)) return false; - if (version<3.0) + if (version &lim) if (!limTorso.getLimits(iTorso,&min,&max)) return false; - if (version<3.0) + if (version &lim) if (!limTorso.getLimits(iTorso,&min,&max)) return false; - if (version<3.0) + if (version1) - version=strtod(getType().substr(1).c_str(),NULL); + version=iKinLimbVersion(getType().substr(1)); else - version=1.0; + version=iKinLimbVersion("1.0"); Matrix H0(4,4); H0.zero(); H0(3,3)=1.0; - if (version<2.0) + if (version2.0) + if (version>iKinLimbVersion("2.0")) { Matrix HN=eye(4,4); HN(0,3)=0.0087; @@ -2744,7 +2866,7 @@ void iCubInertialSensor::allocate(const string &_type) } // further displacement for >= 2.6 - if (version>=2.6) + if (version>=iKinLimbVersion("2.6")) { Matrix HN=zeros(4,4); HN(0,3)=0.0323779; @@ -2777,7 +2899,7 @@ bool iCubInertialSensor::alignJointsBounds(const deque &lim) if (!limTorso.getLimits(iTorso,&min,&max)) return false; - if (version<3.0) + if (version1) - version=strtod(getType().substr(1).c_str(),NULL); + version=iKinLimbVersion(getType().substr(1)); else - version=2.7; + version=iKinLimbVersion("2.7"); Matrix H0(4,4); H0.zero(); diff --git a/src/libraries/iKin/src/iKinSlv.cpp b/src/libraries/iKin/src/iKinSlv.cpp index bc7f56d91c..f75b9a4039 100644 --- a/src/libraries/iKin/src/iKinSlv.cpp +++ b/src/libraries/iKin/src/iKinSlv.cpp @@ -1762,7 +1762,7 @@ PartDescriptor *iCubArmCartesianSolver::getPartDesc(Searchable &options) { type="right"; string part_type=type; - double version=1.0; + iKinLimbVersion version("1.0"); if (options.check("type")) { type=options.find("type").asString(); @@ -1772,7 +1772,7 @@ PartDescriptor *iCubArmCartesianSolver::getPartDesc(Searchable &options) size_t underscore=type.find('_'); if (underscore!=string::npos) - version=strtod(type.substr(underscore+2).c_str(),NULL); + version=iKinLimbVersion(type.substr(underscore+2)); } string robot=options.check("robot",Value("icub")).asString(); @@ -1801,9 +1801,9 @@ PartDescriptor *iCubArmCartesianSolver::getPartDesc(Searchable &options) p->cns=new iCubAdditionalArmConstraints(*static_cast(p->lmb)); p->prp.push_back(optTorso); p->prp.push_back(optArm); - p->rvs.push_back(version<3.0); // torso - p->rvs.push_back(false); // arm - p->num=2; // number of parts + p->rvs.push_back(versionrvs.push_back(false); // arm + p->num=2; // number of parts return p; } @@ -1858,7 +1858,7 @@ PartDescriptor *iCubLegCartesianSolver::getPartDesc(Searchable &options) { type="right"; string part_type=type; - double version=1.0; + iKinLimbVersion version("1.0"); if (options.check("type")) { type=options.find("type").asString(); @@ -1868,7 +1868,7 @@ PartDescriptor *iCubLegCartesianSolver::getPartDesc(Searchable &options) size_t underscore=type.find('_'); if (underscore!=string::npos) - version=strtod(type.substr(underscore+2).c_str(),NULL); + version=iKinLimbVersion(type.substr(underscore+2)); } string robot=options.check("robot",Value("icub")).asString(); diff --git a/src/libraries/icubmod/cartesianController/ClientCartesianController.cpp b/src/libraries/icubmod/cartesianController/ClientCartesianController.cpp index c1e979c89d..8536bb82e1 100644 --- a/src/libraries/icubmod/cartesianController/ClientCartesianController.cpp +++ b/src/libraries/icubmod/cartesianController/ClientCartesianController.cpp @@ -29,7 +29,7 @@ #include -#define CARTCTRL_CLIENT_VER 1.1 +#define CARTCTRL_CLIENT_VER "2.0" #define CARTCTRL_DEFAULT_TMO 0.1 // [s] using namespace std; @@ -120,10 +120,10 @@ bool ClientCartesianController::open(Searchable &config) getInfoHelper(info); if (info.check("server_version")) { - double server_version=info.find("server_version").asFloat64(); + string server_version=info.find("server_version").asString(); if (server_version!=CARTCTRL_CLIENT_VER) { - yError("version mismatch => server(%g) != client(%g); please update accordingly", + yError("version mismatch => server(%s) != client(%s); please update accordingly", server_version,CARTCTRL_CLIENT_VER); close(); return false; diff --git a/src/libraries/icubmod/cartesianController/ServerCartesianController.cpp b/src/libraries/icubmod/cartesianController/ServerCartesianController.cpp index 9e354e03bb..d8b88a9e24 100644 --- a/src/libraries/icubmod/cartesianController/ServerCartesianController.cpp +++ b/src/libraries/icubmod/cartesianController/ServerCartesianController.cpp @@ -30,7 +30,7 @@ #include -#define CARTCTRL_SERVER_VER 1.1 +#define CARTCTRL_SERVER_VER "2.0" #define CARTCTRL_DEFAULT_PER 0.01 // [s] #define CARTCTRL_DEFAULT_TASKVEL_PERFACTOR 4 #define CARTCTRL_DEFAULT_TOL 1e-2 @@ -3487,19 +3487,19 @@ bool ServerCartesianController::getInfo(Bottle &info) Bottle &serverVer=info.addList(); serverVer.addString("server_version"); - serverVer.addFloat64(CARTCTRL_SERVER_VER); + serverVer.addString(CARTCTRL_SERVER_VER); string kinPartStr(kinPart); string type=limbState->getType(); size_t pos=type.find("_v"); - double hwVer=1.0; + iKinLimbVersion hwVer("1.0"); if (pos!=string::npos) - hwVer=strtod(type.substr(pos+2).c_str(),NULL); + hwVer=iKinLimbVersion(type.substr(pos+2)); Bottle &partVer=info.addList(); partVer.addString(kinPartStr+"_version"); - partVer.addFloat64(hwVer); + partVer.addString(hwVer.get_version()); Bottle &partType=info.addList(); partType.addString(kinPartStr+"_type"); diff --git a/src/libraries/icubmod/gazeController/ClientGazeController.cpp b/src/libraries/icubmod/gazeController/ClientGazeController.cpp index dfc0bd4900..d6b46b272a 100644 --- a/src/libraries/icubmod/gazeController/ClientGazeController.cpp +++ b/src/libraries/icubmod/gazeController/ClientGazeController.cpp @@ -25,7 +25,7 @@ #include #include "ClientGazeController.h" -#define GAZECTRL_CLIENT_VER 1.2 +#define GAZECTRL_CLIENT_VER "2.0" #define GAZECTRL_DEFAULT_TMO 0.1 // [s] #define GAZECTRL_ACK Vocab32::encode("ack") #define GAZECTRL_NACK Vocab32::encode("nack") @@ -125,10 +125,10 @@ bool ClientGazeController::open(Searchable &config) getInfoHelper(info); if (info.check("server_version")) { - double server_version=info.find("server_version").asFloat64(); + string server_version=info.find("server_version").asString(); if (server_version!=GAZECTRL_CLIENT_VER) { - yError("version mismatch => server(%g) != client(%g); please update accordingly", + yError("version mismatch => server(%s) != client(%s); please update accordingly", server_version,GAZECTRL_CLIENT_VER); close(); return false; diff --git a/src/modules/actionsRenderingEngine/src/MotorThread.cpp b/src/modules/actionsRenderingEngine/src/MotorThread.cpp index 1b90a680d2..982a377779 100644 --- a/src/modules/actionsRenderingEngine/src/MotorThread.cpp +++ b/src/modules/actionsRenderingEngine/src/MotorThread.cpp @@ -2683,11 +2683,11 @@ bool MotorThread::exploreTorso(Bottle &options) Bottle info; ctrl_gaze->getInfo(info); - double head_version=info.check("head_version",Value(1.0)).asFloat64(); + iKinLimbVersion head_version(info.check("head_version",Value("1.0")).asString()); ostringstream type; type<<(dominant_eye==LEFT?"left":"right"); - if (head_version==2.0) + if (head_version==iKinLimbVersion("2.0")) type<<"_v2"; iCubEye iKinTorso=iCubEye(type.str()); diff --git a/src/modules/cartesianInterfaceControl/CMakeLists.txt b/src/modules/cartesianInterfaceControl/CMakeLists.txt index 7efaa4d055..e2d142b720 100644 --- a/src/modules/cartesianInterfaceControl/CMakeLists.txt +++ b/src/modules/cartesianInterfaceControl/CMakeLists.txt @@ -5,6 +5,6 @@ project(cartesianInterfaceControl) add_executable(${PROJECT_NAME} main.cpp) -target_link_libraries(${PROJECT_NAME} ${YARP_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} iKin ${YARP_LIBRARIES}) install(TARGETS ${PROJECT_NAME} DESTINATION bin) diff --git a/src/modules/cartesianInterfaceControl/main.cpp b/src/modules/cartesianInterfaceControl/main.cpp index 604c920cc7..de9a8e1258 100644 --- a/src/modules/cartesianInterfaceControl/main.cpp +++ b/src/modules/cartesianInterfaceControl/main.cpp @@ -129,6 +129,8 @@ Windows, Linux #include #include +#include + #define MAX_TORSO_PITCH 30.0 // [deg] #define EXECTIME_THRESDIST 0.3 // [m] #define PRINT_STATUS_PER 1.0 // [s] @@ -138,6 +140,7 @@ using namespace yarp::os; using namespace yarp::dev; using namespace yarp::sig; using namespace yarp::math; +using namespace iCub::iKin; /************************************************************************/ @@ -193,11 +196,11 @@ class CtrlThread : public PeriodicThread { Bottle info; iarm->getInfo(info); - double hwver = info.find("arm_version").asFloat64(); - printf("Detected arm kinematics version %g\n", hwver); + iKinLimbVersion hwver(info.find("arm_version").asString()); + printf("Detected arm kinematics version %s\n", hwver.get_version()); map torsoJointsRemap{{ "pitch", 0 }, { "roll", 1 }, { "yaw", 2 }}; - if (hwver >= 3.0) { + if (hwver >= iKinLimbVersion("3.0")) { torsoJointsRemap["roll"] = 0; torsoJointsRemap["pitch"] = 1; } diff --git a/src/modules/iKinGazeCtrl/include/iCub/utils.h b/src/modules/iKinGazeCtrl/include/iCub/utils.h index 8586e147dd..e84b569fb3 100644 --- a/src/modules/iKinGazeCtrl/include/iCub/utils.h +++ b/src/modules/iKinGazeCtrl/include/iCub/utils.h @@ -131,8 +131,6 @@ class ExchangeData std::pair get_gyro(); std::pair get_accel(); - string headVersion2String(); - // data members that do not need protection xdPort *port_xd; string robotName; @@ -142,7 +140,7 @@ class ExchangeData double eyesBoundVer; double gyro_noise_threshold; double stabilizationGain; - double head_version; + iKinLimbVersion head_version; double saccadesInhibitionPeriod; double saccadesActivationAngle; int neckSolveCnt; diff --git a/src/modules/iKinGazeCtrl/src/controller.cpp b/src/modules/iKinGazeCtrl/src/controller.cpp index d6c8d1651c..f4542bdd3d 100644 --- a/src/modules/iKinGazeCtrl/src/controller.cpp +++ b/src/modules/iKinGazeCtrl/src/controller.cpp @@ -34,10 +34,10 @@ Controller::Controller(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData printAccTime(0.0) { // Instantiate objects - neck=new iCubHeadCenter("right_"+commData->headVersion2String()); - eyeL=new iCubEye("left_"+commData->headVersion2String()); - eyeR=new iCubEye("right_"+commData->headVersion2String()); - imu=new iCubInertialSensor(commData->headVersion2String()); + neck=new iCubHeadCenter("right_v"+commData->head_version.get_version()); + eyeL=new iCubEye("left_v"+commData->head_version.get_version()); + eyeR=new iCubEye("right_v"+commData->head_version.get_version()); + imu=new iCubInertialSensor(commData->head_version.get_version()); // remove constraints on the links: logging purpose imu->setAllConstraints(false); diff --git a/src/modules/iKinGazeCtrl/src/localizer.cpp b/src/modules/iKinGazeCtrl/src/localizer.cpp index ef63cee0f5..831ce53c66 100644 --- a/src/modules/iKinGazeCtrl/src/localizer.cpp +++ b/src/modules/iKinGazeCtrl/src/localizer.cpp @@ -29,9 +29,9 @@ Localizer::Localizer(ExchangeData *_commData, const unsigned int _period) : PeriodicThread((double)_period/1000.0), commData(_commData), period(_period) { - iCubHeadCenter eyeC("right_"+commData->headVersion2String()); - eyeL=new iCubEye("left_"+commData->headVersion2String()); - eyeR=new iCubEye("right_"+commData->headVersion2String()); + iCubHeadCenter eyeC("right_v"+commData->head_version.get_version()); + eyeL=new iCubEye("left_v"+commData->head_version.get_version()); + eyeR=new iCubEye("rightv_"+commData->head_version.get_version()); // remove constraints on the links // we use the chains for logging purpose diff --git a/src/modules/iKinGazeCtrl/src/main.cpp b/src/modules/iKinGazeCtrl/src/main.cpp index 4e0672605c..bb07b64410 100644 --- a/src/modules/iKinGazeCtrl/src/main.cpp +++ b/src/modules/iKinGazeCtrl/src/main.cpp @@ -578,7 +578,7 @@ Windows, Linux #include #include -#define GAZECTRL_SERVER_VER 1.2 +#define GAZECTRL_SERVER_VER "2.0" using namespace std; using namespace yarp::os; @@ -765,11 +765,11 @@ class GazeModule: public RFModule Bottle &serverVer=info.addList(); serverVer.addString("server_version"); - serverVer.addFloat64(GAZECTRL_SERVER_VER); + serverVer.addString(GAZECTRL_SERVER_VER); Bottle &headVer=info.addList(); headVer.addString("head_version"); - headVer.addFloat64(commData.head_version); + headVer.addString(commData.head_version.get_version()); Bottle &minVer=info.addList(); minVer.addString("min_allowed_vergence"); @@ -1067,25 +1067,25 @@ class GazeModule: public RFModule } /************************************************************************/ - double constrainHeadVersion(const double ver_in) + iKinLimbVersion constrainHeadVersion(const iKinLimbVersion &ver_in) { // std::map is ordered based on std::less - map d; - d[fabs(1.0-ver_in)]=1.0; - d[fabs(2.0-ver_in)]=2.0; - d[fabs(2.5-ver_in)]=2.5; - d[fabs(2.6-ver_in)]=2.6; - d[fabs(2.7-ver_in)]=2.7; - d[fabs(2.8-ver_in)]=2.8; - d[fabs(2.10-ver_in)]=2.10; - d[fabs(3.0-ver_in)]=3.0; - d[fabs(3.1-ver_in)]=3.1; - - double ver_out=d.begin()->second; + map d; + d[iKinLimbVersion("1.0")-ver_in]=iKinLimbVersion("1.0"); + d[iKinLimbVersion("2.0")-ver_in]=iKinLimbVersion("2.0"); + d[iKinLimbVersion("2.5")-ver_in]=iKinLimbVersion("2.5"); + d[iKinLimbVersion("2.6")-ver_in]=iKinLimbVersion("2.6"); + d[iKinLimbVersion("2.7")-ver_in]=iKinLimbVersion("2.7"); + d[iKinLimbVersion("2.8")-ver_in]=iKinLimbVersion("2.8"); + d[iKinLimbVersion("2.10")-ver_in]=iKinLimbVersion("2.10"); + d[iKinLimbVersion("3.0")-ver_in]=iKinLimbVersion("3.0"); + d[iKinLimbVersion("3.1")-ver_in]=iKinLimbVersion("3.1"); + + auto ver_out=d.begin()->second; if (ver_out!=ver_in) { - yWarning("Unknown \"head_version\" %g requested => used \"head_version\" %g instead", - ver_in,ver_out); + yWarning("Unknown \"head_version\" %s requested => used \"head_version\" %s instead", + ver_in.get_version(),ver_out.get_version()); } return ver_out; @@ -1142,7 +1142,7 @@ class GazeModule: public RFModule commData.robotName=rf.check("robot",Value("icub")).asString(); commData.eyeTiltLim[0]=eyeTiltGroup.check("min",Value(-20.0)).asFloat64(); commData.eyeTiltLim[1]=eyeTiltGroup.check("max",Value(15.0)).asFloat64(); - commData.head_version=constrainHeadVersion(rf.check("head_version",Value(1.0)).asFloat64()); + commData.head_version=constrainHeadVersion(iKinLimbVersion(rf.check("head_version",Value("1.0")).asString())); commData.verbose=rf.check("verbose"); commData.saccadesOn=(rf.check("saccades",Value("on")).asString()=="on"); commData.neckPosCtrlOn=(rf.check("neck_position_control",Value("on")).asString()=="on"); @@ -1177,7 +1177,7 @@ class GazeModule: public RFModule commData.rf_tweak.setDefaultConfigFile(commData.tweakFile.c_str()); commData.rf_tweak.configure(0,nullptr); - yInfo("Controller configured for head version %g",commData.head_version); + yInfo("Controller configured for head version %s",commData.head_version.get_version()); commData.localStemName="/"+ctrlName; string remoteHeadName="/"+commData.robotName+"/"+headName; diff --git a/src/modules/iKinGazeCtrl/src/solver.cpp b/src/modules/iKinGazeCtrl/src/solver.cpp index 313196222b..091d3cc2f5 100644 --- a/src/modules/iKinGazeCtrl/src/solver.cpp +++ b/src/modules/iKinGazeCtrl/src/solver.cpp @@ -32,10 +32,10 @@ EyePinvRefGen::EyePinvRefGen(PolyDriver *_drvTorso, PolyDriver *_drvHead, Ts(_period/1000.0), counterRotGain(_counterRotGain) { // Instantiate objects - neck=new iCubHeadCenter("right_"+commData->headVersion2String()); - eyeL=new iCubEye("left_"+commData->headVersion2String()); - eyeR=new iCubEye("right_"+commData->headVersion2String()); - imu=new iCubInertialSensor(commData->headVersion2String()); + neck=new iCubHeadCenter("right_v"+commData->head_version.get_version()); + eyeL=new iCubEye("left_v"+commData->head_version.get_version()); + eyeR=new iCubEye("right_v"+commData->head_version.get_version()); + imu=new iCubInertialSensor(commData->head_version.get_version()); // remove constraints on the links: logging purpose imu->setAllConstraints(false); @@ -465,10 +465,10 @@ Solver::Solver(PolyDriver *_drvTorso, PolyDriver *_drvHead, ExchangeData *_commD ctrl(_ctrl), period(_period), Ts(_period/1000.0) { // Instantiate objects - neck=new iCubHeadCenter("right_"+commData->headVersion2String()); - eyeL=new iCubEye("left_"+commData->headVersion2String()); - eyeR=new iCubEye("right_"+commData->headVersion2String()); - imu=new iCubInertialSensor(commData->headVersion2String()); + neck=new iCubHeadCenter("right_v"+commData->head_version.get_version()); + eyeL=new iCubEye("left_v"+commData->head_version.get_version()); + eyeR=new iCubEye("right_v"+commData->head_version.get_version()); + imu=new iCubInertialSensor(commData->head_version.get_version()); torsoVel=new AWLinEstimator(16,0.5); // remove constraints on the links: logging purpose diff --git a/src/modules/iKinGazeCtrl/src/utils.cpp b/src/modules/iKinGazeCtrl/src/utils.cpp index 0267b08968..80d20c58d3 100644 --- a/src/modules/iKinGazeCtrl/src/utils.cpp +++ b/src/modules/iKinGazeCtrl/src/utils.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include #include @@ -157,7 +156,7 @@ ExchangeData::ExchangeData() robotName=""; localStemName=""; - head_version=1.0; + head_version=iKinLimbVersion("1.0"); tweakOverwrite=true; tweakFile=""; iGyro = nullptr; @@ -375,13 +374,6 @@ std::pair ExchangeData::get_accel() { return ret; // RVO } -/************************************************************************/ -string ExchangeData::headVersion2String() -{ - ostringstream str; - str<<"v"<getLimits(i,&min,&max)) { - if (commData->head_version<3.0) + if (commData->head_versiongetEncodersTimed(fb.data(),stamps.data())) { for (int i=0; ihead_version<3.0)?fb[nJointsTorso-1-i]:fb[i]); + fbTorso[i]=CTRL_DEG2RAD*((commData->head_version