Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Support of 7 joints in the mk3 hand #744

Merged
merged 4 commits into from
Apr 27, 2021
Merged

Support of 7 joints in the mk3 hand #744

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
ce3a610
added parsing of eomn_serv_MC_mc4pluspmc
marcoaccame Feb 11, 2021
d1db97d
fixed initialization of service params for the case of eomn_serv_MC_m…
marcoaccame Apr 12, 2021
b2eef4f
fixed typo in comment
marcoaccame Apr 12, 2021
b4264b7
added dependency vs icub-firmware-shared version 1.19.2
marcoaccame Apr 26, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion conf/iCubFindDependencies.cmake
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
113 changes: 110 additions & 3 deletions src/libraries/icubmod/embObjLib/serviceParser.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

}
Expand Down Expand Up @@ -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;
Expand All @@ -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)
{
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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; i<mc_service.properties.poslocations.size(); i++)
{
pos->boardInfo.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<numofjomos; i++)
{
eOmc_jomo_descriptor_t jomodes = {};

// 1. actuator is on pwm for the first 4 and on pmc on trh last 3: we need the port
if(mc_service.properties.actuators[i].type == eomc_act_pwm)
{
jomodes.actuator.pwm.port = mc_service.properties.actuators[i].desc.pwm.port;
}
else if(mc_service.properties.actuators[i].type == eomc_act_pmc)
{
jomodes.actuator.pmc.canloc.port = mc_service.properties.actuators[i].desc.pmc.canloc.port;
jomodes.actuator.pmc.canloc.addr = mc_service.properties.actuators[i].desc.pmc.canloc.addr;
jomodes.actuator.pmc.canloc.insideindex = mc_service.properties.actuators[i].desc.pmc.canloc.insideindex;
}
else
{
yError() << "ServiceParser::parseService() unknown vatuator value for eomn_serv_MC_mc4pluspmc mode";
}

// 2. encoder1 is ...
jomodes.encoder1.type = mc_service.properties.encoder1s[i].desc.type;
jomodes.encoder1.port = mc_service.properties.encoder1s[i].desc.port;
jomodes.encoder1.pos = mc_service.properties.encoder1s[i].desc.pos;

// 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;

eo_array_PushBack(arrayofjomos, &jomodes);
}

// ok, everything is done
ret = true;

} break;

case eomn_serv_MC_generic:
{
yWarning() << "ServiceParser::parseService() eomn_serv_MC_generic detected. set type = eomn_serv_MC_generic to let the remote board configure itself according to its IP address";
Expand Down
34 changes: 33 additions & 1 deletion src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -579,7 +579,9 @@ bool embObjMotionControl::updatedJointsetsCfgWithControlInfo()

bool embObjMotionControl::saveCouplingsData(void)
{
eOmc_4jomo_coupling_t *jc_dest;
eOmc_4jomo_coupling_t *jc_dest;

static eOmc_4jomo_coupling_t dummyjomocoupling = {};

switch(serviceConfig.ethservice.configuration.type)
{
Expand Down Expand Up @@ -607,6 +609,11 @@ bool embObjMotionControl::saveCouplingsData(void)
{
jc_dest = &(serviceConfig.ethservice.configuration.data.mc.mc4plusfaps.jomocoupling);

} break;
case eomn_serv_MC_mc4pluspmc:
{
jc_dest = &dummyjomocoupling; // this mode does not have a coupling as it is w/ 7 independent joints

} break;
case eomn_serv_MC_mc4:
{
Expand All @@ -623,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
Expand Down Expand Up @@ -4733,6 +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)
)
return true;
else
Expand Down