From ce3a6103e68ba4946ec07290072140bc59c1704a Mon Sep 17 00:00:00 2001 From: "marco.accame" Date: Thu, 11 Feb 2021 18:45:08 +0100 Subject: [PATCH 1/4] added parsing of eomn_serv_MC_mc4pluspmc --- .../icubmod/embObjLib/serviceParser.cpp | 115 +++++++++++++++++- .../embObjMotionControl.cpp | 10 +- 2 files changed, 120 insertions(+), 5 deletions(-) diff --git a/src/libraries/icubmod/embObjLib/serviceParser.cpp b/src/libraries/icubmod/embObjLib/serviceParser.cpp index a8e621ac82..d23ed0b4ed 100755 --- a/src/libraries/icubmod/embObjLib/serviceParser.cpp +++ b/src/libraries/icubmod/embObjLib/serviceParser.cpp @@ -439,13 +439,13 @@ bool ServiceParser::convert(std::string const &fromstring, eObrd_location_t &loc { location.extcan.port = (1 == bus) ? (eOcanport1) : (eOcanport2); location.extcan.addr = adr; - if((0 != sub) && (1 != sub)) + if((eobrd_caninsideindex_first != sub) && (eobrd_caninsideindex_second != sub) && (eobrd_caninsideindex_third != sub) && (eobrd_caninsideindex_fourth != sub)) { - yWarning() << "ServiceParser::convert():" << t << "is not a legal string for a eObrd_location_t because in CANx:adr:SUB, SUB address must be in range [0, 1]"; + yWarning() << "ServiceParser::convert():" << t << "is not a legal string for a eObrd_location_t because in CANx:adr:SUB, SUB address must be in range [0, 3]"; formaterror = true; return false; } - location.extcan.index = (0 == sub) ? (eobrd_caninsideindex_first) : (eobrd_caninsideindex_second); + location.extcan.index = sub; } } @@ -2102,6 +2102,7 @@ bool ServiceParser::parse_actuator_port(std::string const &fromstring, eObrd_eth case eomc_act_foc: case eomc_act_mc4: + case eomc_act_pmc: { // read it as a CAN address eObrd_location_t loc; @@ -2128,6 +2129,15 @@ bool ServiceParser::parse_actuator_port(std::string const &fromstring, eObrd_eth ret = false; } } + else if(eomc_act_pmc == type) + { + if((eobrd_place_can != loc.any.place)) + { + yWarning() << "ServiceParser::parse_actuator_port():" << t << "is not a legal string for a eomc_act_pmc location"; + formaterror = true; + ret = false; + } + } if(false == ret) { @@ -2684,6 +2694,38 @@ bool ServiceParser::check_motion(Searchable &config) } break; + case eomn_serv_MC_mc4pluspmc: + { + // must have: ETHBOARD, CANBOARDS, POS, JOINTMAPPING + + itisoksofar = true; + + if(false == has_PROPERTIES_ETHBOARD) + { + yError() << "ServiceParser::check_motion() cannot find PROPERTIES.ETHBOARD for type" << eomn_servicetype2string(mc_service.type); + itisoksofar = false; + } + + if(false == has_PROPERTIES_CANBOARDS) + { + yError() << "ServiceParser::check_motion() cannot find PROPERTIES.CANBOARDS for type" << eomn_servicetype2string(mc_service.type); + itisoksofar = false; + } + + if(false == has_PROPERTIES_POS) + { + yError() << "ServiceParser::check_motion() cannot find PROPERTIES.POS for type" << eomn_servicetype2string(mc_service.type); + itisoksofar = false; + } + + if(false == has_PROPERTIES_JOINTMAPPING) + { + yError() << "ServiceParser::check_motion() cannot find PROPERTIES.JOINTMAPPING for type" << eomn_servicetype2string(mc_service.type); + itisoksofar = false; + } + + } break; + default: { yError() << "ServiceParser::check_motion() has found an unknown type" << eomn_servicetype2string(mc_service.type); @@ -3787,7 +3829,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig) jomodes.encoder1.port = mc_service.properties.encoder1s[i].desc.port; jomodes.encoder1.pos = mc_service.properties.encoder1s[i].desc.pos; - // 3. encoder2 is ... + // 3. encoder2 src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cppis ... jomodes.encoder2.type = mc_service.properties.encoder2s[i].desc.type; jomodes.encoder2.port = mc_service.properties.encoder2s[i].desc.port; jomodes.encoder2.pos = mc_service.properties.encoder2s[i].desc.pos; @@ -3859,6 +3901,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig) } break; + case eomn_serv_MC_mc4plusfaps: { eOmn_serv_config_data_mc_mc4plusfaps_t *data_mc = &(mcconfig.ethservice.configuration.data.mc.mc4plusfaps); @@ -3910,6 +3953,70 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig) } break; + + case eomn_serv_MC_mc4pluspmc: + { + eOmn_serv_config_data_mc_mc4pluspmc_t *data_mc = &(mcconfig.ethservice.configuration.data.mc.mc4pluspmc); + + // 1. ->pos + eOmn_serv_config_data_as_pos_t *pos = &data_mc->pos; + + for(size_t i=0; iboardInfo.canloc[i].port = mc_service.properties.poslocations[i].port; + pos->boardInfo.canloc[i].addr = mc_service.properties.poslocations[i].addr; + pos->boardInfo.canloc[i].insideindex = mc_service.properties.poslocations[i].insideindex; + } + + + pos->version.firmware.major = mc_service.properties.canboards.at(0).firmware.major; + pos->version.firmware.minor = mc_service.properties.canboards.at(0).firmware.minor; + pos->version.firmware.build = mc_service.properties.canboards.at(0).firmware.build; + pos->version.protocol.major = mc_service.properties.canboards.at(0).protocol.major; + pos->version.protocol.minor = mc_service.properties.canboards.at(0).protocol.minor; + + // 2. ->arrayofjomodescriptors + EOarray *arrayofjomos = eo_array_New(7, sizeof(eOmc_jomo_descriptor_t), &data_mc->arrayof7jomodescriptors); + size_t numofjomos = mc_service.properties.numofjoints; + + for(size_t i=0; i Date: Mon, 12 Apr 2021 11:18:19 +0200 Subject: [PATCH 2/4] fixed initialization of service params for the case of eomn_serv_MC_mc4pluspmc --- .../embObjMotionControl.cpp | 26 ++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp index c7077d51fa..faf42c5cb7 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp @@ -630,6 +630,30 @@ bool embObjMotionControl::saveCouplingsData(void) } } + + // this mode does not use jointsets but only eOmc_jointset_configuration_t + if(eomn_serv_MC_mc4pluspmc == serviceConfig.ethservice.configuration.type) + { + eOmc_arrayof_7jointsetconfig_t *arrayof7jset = &(serviceConfig.ethservice.configuration.data.mc.mc4pluspmc.arrayof7jointsets); + EOarray *array = eo_array_New(7, sizeof(eOmc_jointset_configuration_t), arrayof7jset); + // must initialise each entry of array + + //for(size_t s=0; s< _jsets.size(); s++) + for(size_t s=0; s<4; s++) + { + eOmc_jointset_configuration_t* cfg_ptr = _jsets[s].getConfiguration(); + eo_array_PushBack(array, cfg_ptr); + } + for(size_t e=0; e<3; e++) + { + eOmc_jointset_configuration_t cfg = {0}; + eo_array_PushBack(array, &cfg); + } + + return true; + } + + memset(jc_dest, 0, sizeof(eOmc_4jomo_coupling_t)); //I need to initialize all elements of joint2set with "eomc_jointSetNum_none": it is used by fw to get num of setBemfParamRaw @@ -4740,7 +4764,7 @@ bool embObjMotionControl::iNeedCouplingsInfo(void) (mc_serv_type == eomn_serv_MC_mc4plusmais) || (mc_serv_type == eomn_serv_MC_mc2pluspsc) || (mc_serv_type == eomn_serv_MC_mc4plusfaps) -// || (mc_serv_type == eomn_serv_MC_mc4pluspmc) + || (mc_serv_type == eomn_serv_MC_mc4pluspmc) ) return true; else From b2eef4fe30a295602655f30507557a97dfd51afc Mon Sep 17 00:00:00 2001 From: "marco.accame" Date: Mon, 12 Apr 2021 11:27:12 +0200 Subject: [PATCH 3/4] fixed typo in comment --- src/libraries/icubmod/embObjLib/serviceParser.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libraries/icubmod/embObjLib/serviceParser.cpp b/src/libraries/icubmod/embObjLib/serviceParser.cpp index d23ed0b4ed..fb6a517b6e 100755 --- a/src/libraries/icubmod/embObjLib/serviceParser.cpp +++ b/src/libraries/icubmod/embObjLib/serviceParser.cpp @@ -3829,7 +3829,7 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig) jomodes.encoder1.port = mc_service.properties.encoder1s[i].desc.port; jomodes.encoder1.pos = mc_service.properties.encoder1s[i].desc.pos; - // 3. encoder2 src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cppis ... + // 3. encoder2 is ... jomodes.encoder2.type = mc_service.properties.encoder2s[i].desc.type; jomodes.encoder2.port = mc_service.properties.encoder2s[i].desc.port; jomodes.encoder2.pos = mc_service.properties.encoder2s[i].desc.pos; From b4264b7e7dddf71424a926dc9d73cb8394728364 Mon Sep 17 00:00:00 2001 From: "marco.accame" Date: Mon, 26 Apr 2021 15:30:42 +0200 Subject: [PATCH 4/4] added dependency vs icub-firmware-shared version 1.19.2 --- conf/iCubFindDependencies.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/iCubFindDependencies.cmake b/conf/iCubFindDependencies.cmake index 599b4817e8..afcbcbd198 100644 --- a/conf/iCubFindDependencies.cmake +++ b/conf/iCubFindDependencies.cmake @@ -34,7 +34,7 @@ message(STATUS "CMake modules directory: ${CMAKE_MODULE_PATH}") find_package(icub_firmware_shared QUIET) if(icub_firmware_shared_FOUND) # icub-firmware-shared 4.0.7 was actually a wrong version exported by icub-firmware-shared <= 1.15 - if(icub_firmware_shared_VERSION VERSION_GREATER 4 OR icub_firmware_shared_VERSION VERSION_LESS 1.19.1) + if(icub_firmware_shared_VERSION VERSION_GREATER 4 OR icub_firmware_shared_VERSION VERSION_LESS 1.19.2) message(FATAL_ERROR "An old version of icub-firmware-shared has been detected, but at least 1.18.100 is required") endif() endif()