From 6376390d867d57ba4ae4e55fab4467a9a256b1e0 Mon Sep 17 00:00:00 2001 From: Marco Accame Date: Tue, 5 Mar 2024 09:48:14 +0100 Subject: [PATCH] The `amc` board uses `ICC` and `CAN` to move the wrist of `ergoCub` (#948) --- conf/iCubFindDependencies.cmake | 2 +- .../icubmod/embObjLib/serviceParser.cpp | 254 ++++++++++++++---- .../icubmod/embObjLib/serviceParser.h | 3 + .../embObjMotionControl.cpp | 82 +++--- 4 files changed, 255 insertions(+), 86 deletions(-) diff --git a/conf/iCubFindDependencies.cmake b/conf/iCubFindDependencies.cmake index 53def9b566..480da9fb6c 100644 --- a/conf/iCubFindDependencies.cmake +++ b/conf/iCubFindDependencies.cmake @@ -64,7 +64,7 @@ checkandset_dependency(IPOPT) checkandset_dependency(OpenCV) checkandset_dependency(Qt5) -set(MINIMUM_REQUIRED_icub_firmware_shared_VERSION 1.38.0) +set(MINIMUM_REQUIRED_icub_firmware_shared_VERSION 1.38.1) if(icub_firmware_shared_FOUND AND ICUB_USE_icub_firmware_shared) if(icub_firmware_shared_VERSION VERSION_LESS ${MINIMUM_REQUIRED_icub_firmware_shared_VERSION}) diff --git a/src/libraries/icubmod/embObjLib/serviceParser.cpp b/src/libraries/icubmod/embObjLib/serviceParser.cpp index 1320763392..73f3bbcc19 100755 --- a/src/libraries/icubmod/embObjLib/serviceParser.cpp +++ b/src/libraries/icubmod/embObjLib/serviceParser.cpp @@ -508,6 +508,116 @@ bool ServiceParser::convert(eObrd_location_t const &loc, char *str, int len) return true; } +bool ServiceParser::convert(std::string const &fromstring, eOlocation_t &location, bool &formaterror) +{ + // it is actually a micro-parser: PRE-num + + const char *t = fromstring.c_str(); + int len = strlen(t); + + if(len > 15) + { + yWarning() << "ServiceParser::convert():" << t << "is not a legal string for a eOlocation_t because it is too long with size = " << len; + formaterror = true; + return false; + } + + char prefix[16] = {0}; + sscanf(t, "%3c", prefix); + if(0 == strcmp(prefix, "LOC")) + { + int adr = 0; + sscanf(t, "%3c:%d", prefix, &adr); + location.bus = eobus_local; + location.ffu = 0; + location.adr = adr; + } + else if((0 == strcmp(prefix, "CAN")) || (0 == strcmp(prefix, "ICC"))) + { + int bus = 0; + int adr = 0; + int numberofreaditems = sscanf(t, "%3c%d:%d", prefix, &bus, &adr); + + if(3 != numberofreaditems) + { + yWarning() << "ServiceParser::convert():" << t << "is not a legal string for a eOlocation_t because we dont have correct number of sections separated by :"; + formaterror = true; + return false; + } + + // verify bus being eitehr 1 or 2, and adr being 1 ----- 14 + if((1 != bus) && (2 != bus)) + { + yWarning() << "ServiceParser::convert():" << t << "is not a legal string for a eOlocation_t because we can have either ---1 or ---2"; + formaterror = true; + return false; + } + + if((0 == adr) || (adr > 14)) + { + yWarning() << "ServiceParser::convert():" << t << "is not a legal string for a eOlocation_t because address is not in range [1, 14]"; + formaterror = true; + return false; + } + + location.ffu = 0; + if(0 == strcmp(prefix, "CAN")) + { + location.bus = (1 == bus) ? eobus_can1 : eobus_can2; + } + else if(0 == strcmp(prefix, "ICC")) + { + location.bus = (1 == bus) ? eobus_icc1 : eobus_icc2; + } + + location.adr = adr; + + } + else + { + yWarning() << "ServiceParser::convert():" << t << "is not a legal string for a eOlocation_t because it does not begin with LOC, CAN, ICC"; + formaterror = true; + return false; + } + + return true; +} + +bool ServiceParser::convert(eOlocation_t const &loc, char *str, int len) +{ + if((NULL == str) || (0 == len)) + { + return false; + } + + if(eobus_can1 == loc.bus) + { + snprintf(str, len, "CAN1:%d", loc.adr); + } + else if(eobus_can2 == loc.bus) + { + snprintf(str, len, "CAN2:%d", loc.adr); + } + else if(eobus_local == loc.bus) + { + snprintf(str, len, "LOC:%d", loc.adr); + } + else if(eobus_icc1 == loc.bus) + { + snprintf(str, len, "ICC1:%d", loc.adr); + } + else if(eobus_icc2 == loc.bus) + { + snprintf(str, len, "ICC2:%d", loc.adr); + } + else + { + return false; + } + + return true; +} + bool ServiceParser::convert(eObrd_canlocation_t const &canloc, char *str, int len) { if((NULL == str) || (0 == len)) @@ -2243,7 +2353,6 @@ 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; @@ -2270,15 +2379,6 @@ 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) { @@ -2333,10 +2433,32 @@ bool ServiceParser::parse_actuator_port(std::string const &fromstring, eObrd_eth } } + } break; + case eomc_act_advfoc: + { + // read it as a location + eOlocation_t loc {eobus_none, 0, 0}; + bool result = convert(fromstring, loc, formaterror); + ret = true; + if(false == result) + { + yWarning() << "ServiceParser::parse_actuator_port():" << t << "is not a legal string for a eOlocation_t required by eomc_act_advfoc"; + formaterror = true; + ret = false; + } + + if(false == ret) + { + return ret; + } + + // copy into todes.gen as it is uded by advfoc + todes.gen.location = loc; } break; + } return ret; @@ -2922,9 +3044,9 @@ bool ServiceParser::check_motion(Searchable &config) } break; - case eomn_serv_MC_mc4pluspmc: + case eomn_serv_MC_advfoc: { - // must have: ETHBOARD, CANBOARDS, POS, JOINTMAPPING + // must have: ETHBOARD, CANBOARDS, JOINTMAPPING itisoksofar = true; @@ -2940,12 +3062,6 @@ bool ServiceParser::check_motion(Searchable &config) 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); @@ -3475,6 +3591,10 @@ bool ServiceParser::check_motion(Searchable &config) yError() << "ServiceParser::check_motion() cannot find PROPERTIES.JOINTMAPPING.ACTUATOR.type"; return false; } + + Bottle b_PROPERTIES_JOINTMAPPING_ACTUATOR_onboard = Bottle(b_PROPERTIES_JOINTMAPPING_ACTUATOR.findGroup("onboard")); + // mandatory only for advfoc + Bottle b_PROPERTIES_JOINTMAPPING_ACTUATOR_port = Bottle(b_PROPERTIES_JOINTMAPPING_ACTUATOR.findGroup("port")); if(b_PROPERTIES_JOINTMAPPING_ACTUATOR_port.isNull()) { @@ -3623,6 +3743,27 @@ bool ServiceParser::check_motion(Searchable &config) mc_service.properties.mc4joints.push_back(item); } + if(eomn_serv_MC_advfoc == mc_service.type) + { + if(b_PROPERTIES_JOINTMAPPING_ACTUATOR_onboard.isNull()) + { + yError() << "ServiceParser::check_motion() cannot find PROPERTIES.JOINTMAPPING.ACTUATOR.onboard for eomn_serv_MC_advfoc"; + return false; + } + + if(act.type != eomc_act_advfoc) + { + yError() << "ServiceParser::check_motion() detectde that PROPERTIES.JOINTMAPPING.ACTUATOR.type for eomn_serv_MC_advfoc id not advfoc"; + return false; + } + + act.advdescr.type = eomc_act_advfoc; + act.advdescr.location = act.desc.gen.location; + eObrd_cantype_t bb {}; + convert(b_PROPERTIES_JOINTMAPPING_ACTUATOR_onboard.get(i+1).asString(), bb, formaterror); + act.advdescr.board.type = bb; + } + // encoder1s ... @@ -4239,57 +4380,64 @@ bool ServiceParser::parseService(Searchable &config, servConfigMC_t &mcconfig) } break; + case eomn_serv_MC_advfoc: + { + eOmn_serv_config_data_mc_advfoc_t *data_mc = &(mcconfig.ethservice.configuration.data.mc.advfoc); - 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; iconfig.boardconfig[0].canloc.port = mc_service.properties.poslocations[i].port; - pos->config.boardconfig[0].canloc.addr = mc_service.properties.poslocations[i].addr; - pos->config.boardconfig[0].canloc.insideindex = mc_service.properties.poslocations[i].insideindex; - } - - - pos->config.boardconfig[0].boardinfo.firmware.major = mc_service.properties.canboards.at(0).firmware.major; - pos->config.boardconfig[0].boardinfo.firmware.minor = mc_service.properties.canboards.at(0).firmware.minor; - pos->config.boardconfig[0].boardinfo.firmware.build = mc_service.properties.canboards.at(0).firmware.build; - pos->config.boardconfig[0].boardinfo.protocol.major = mc_service.properties.canboards.at(0).protocol.major; - pos->config.boardconfig[0].boardinfo.protocol.minor = mc_service.properties.canboards.at(0).protocol.minor; + ret = true; - // 2. ->arrayofjomodescriptors - EOarray *arrayofjomos = eo_array_New(7, sizeof(eOmc_jomo_descriptor_t), &data_mc->arrayof7jomodescriptors); + // 1. ->eOmc_arrayof_4advjomodescriptors_t + EOarray *arrayofjomos = eo_array_New(4, sizeof(eOmc_adv_jomo_descriptor_t), &data_mc->arrayof4advjomodescriptors); size_t numofjomos = mc_service.properties.numofjoints; for(size_t i=0; ijsetcfg[s]), cfg_ptr, sizeof(eOmc_jointset_configuration_t)); } + + if(eomn_serv_MC_advfoc == serviceConfig.ethservice.configuration.type) + { + // i will copy data from jc_dest to the effective destination + eOmc_adv4jomo_coupling_t *ajc = &serviceConfig.ethservice.configuration.data.mc.advfoc.adv4jomocoupling; + ajc->type = eommccoupling_traditional4x4; + // i copy some fields as they are + std::memmove(&ajc->data.coupling4x4.joint2set[0], &jc_dest->joint2set[0], 4*sizeof(uint8_t)); + std::memmove(&ajc->data.coupling4x4.jsetcfg[0], &jc_dest->jsetcfg[0], 4*sizeof(eOmc_jointset_configuration_t)); + std::memmove(&ajc->data.coupling4x4.joint2motor, &jc_dest->joint2motor, sizeof(eOmc_4x4_matrix_t)); + std::memmove(&ajc->data.coupling4x4.motor2joint, &jc_dest->motor2joint, sizeof(eOmc_4x4_matrix_t)); + // and i will copy only 4x4 from one field + for(uint8_t r=0; r<4; r++) + { + for(uint8_t c=0; c<4; c++) + { + ajc->data.coupling4x4.encoder2joint4x4[r][c] = jc_dest->encoder2joint[r][c]; + } + } + } + return true; } @@ -860,7 +862,7 @@ bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config) { bool lowLevPidisMandatory = false; - if(serviceConfig.ethservice.configuration.type == eomn_serv_MC_foc) + if((serviceConfig.ethservice.configuration.type == eomn_serv_MC_foc) || (serviceConfig.ethservice.configuration.type == eomn_serv_MC_advfoc)) { lowLevPidisMandatory = true; } @@ -974,10 +976,26 @@ bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config) return false; } - /////// [2FOC] or [AMCBLDC] - if(serviceConfig.ethservice.configuration.type == eomn_serv_MC_foc) + /////// [2FOC] or [AMCBLDC] or [ADVFOC_COMMON] + eOmn_serv_type_t servtype = static_cast(serviceConfig.ethservice.configuration.type); + + if((eomn_serv_MC_foc == servtype) || (eomn_serv_MC_advfoc == servtype)) { - std::string groupName = (static_cast(serviceConfig.ethservice.configuration.data.mc.foc_based.type) == eobrd_foc) ? "2FOC" : "AMCBLDC"; + std::string groupName = {}; + + if(eomn_serv_MC_foc == servtype) + { + // in here the name of the group depends on the configured board + eObrd_type_t brd = static_cast(serviceConfig.ethservice.configuration.data.mc.foc_based.type); + groupName = (eobrd_foc == brd) ? "2FOC" : "AMCBLDC"; + } + else if(eomn_serv_MC_advfoc == servtype) + { + // but in here we may have multiple boards, so ... it is better to use a generic name + // ADVFOC with multiple columns, one for each motor + groupName = "ADVFOC"; + } + if(!_mcparser->parseFocGroup(config, _foc_based_info, groupName, _temperatureSensorsVector)) return false; @@ -5108,8 +5126,8 @@ bool embObjMotionControl::iNeedCouplingsInfo(void) (mc_serv_type == eomn_serv_MC_mc4plus) || (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_mc4plusfaps) || + (mc_serv_type == eomn_serv_MC_advfoc) ) return true; else