diff --git a/src/libraries/icubmod/CMakeLists.txt b/src/libraries/icubmod/CMakeLists.txt index a2e3169344..932dde9f0a 100644 --- a/src/libraries/icubmod/CMakeLists.txt +++ b/src/libraries/icubmod/CMakeLists.txt @@ -55,6 +55,7 @@ yarp_begin_plugin_library(icubmod QUIET) add_subdirectory(rawValuesPublisherServer) add_subdirectory(fakeRawValuesPublisher) add_subdirectory(couplingICubHandMk2) + add_subdirectory(couplingICubEye) if (ICUB_ICUBINTERFACE_EXPERIMENTAL) add_subdirectory(imu3DM_GX3) endif() diff --git a/src/libraries/icubmod/couplingICubEye/CMakeLists.txt b/src/libraries/icubmod/couplingICubEye/CMakeLists.txt new file mode 100644 index 0000000000..e99841fbf9 --- /dev/null +++ b/src/libraries/icubmod/couplingICubEye/CMakeLists.txt @@ -0,0 +1,42 @@ +# Copyright (C) 2006-2024 Istituto Italiano di Tecnologia (IIT) +# All rights reserved. +# +# This software may be modified and distributed under the terms of the +# BSD-3-Clause license. See the accompanying LICENSE file for details. + +yarp_prepare_plugin(couplingICubEye + CATEGORY device + TYPE CouplingICubEye + INCLUDE CouplingICubEye.h + GENERATE_PARSER + DEFAULT ON) + +if(ENABLE_couplingICubEye) + option(ALLOW_DEVICE_PARAM_PARSER_GENERATION "Generate the param parser for couplingICubEye device" OFF) + yarp_add_plugin(yarp_couplingICubEye) + + if(MSVC) + add_definitions(-D_USE_MATH_DEFINES) + endif() + + target_sources(yarp_couplingICubEye PRIVATE CouplingICubEye.cpp + CouplingICubEye.h + CouplingICubEye_ParamsParser.cpp + CouplingICubEye_ParamsParser.h) + + target_link_libraries(yarp_couplingICubEye PRIVATE YARP::YARP_os + YARP::YARP_dev) + list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS YARP_os + YARP_dev) + + yarp_install(TARGETS yarp_couplingICubEye + EXPORT YARP_${YARP_PLUGIN_MASTER} + COMPONENT ${YARP_PLUGIN_MASTER} + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}) + + set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE) + + set_property(TARGET yarp_couplingICubEye PROPERTY FOLDER "Plugins/Device") +endif() \ No newline at end of file diff --git a/src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp new file mode 100644 index 0000000000..9cf5823892 --- /dev/null +++ b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp @@ -0,0 +1,174 @@ +/* + * Copyright (C) 2006-2024 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#include "CouplingICubEye.h" +#include +#include +#include +#include + + +YARP_LOG_COMPONENT(COUPLINGICUBEYE, "yarp.device.couplingICubEye") + + +bool CouplingICubEye::populateCouplingParameters() { + yarp::sig::VectorOf coupled_physical_joints{1, 2}; + yarp::sig::VectorOf coupled_actuated_axes{1, 2}; + std::vector> physical_joint_limits; + + physical_joint_limits.resize(m_jointNames.size()); + for (int i = 0; i< m_jointNames.size(); i++) + { + physical_joint_limits[i] = std::make_pair(m_LIMITS_jntPosMin[i], m_LIMITS_jntPosMax[i]); + } + initialise(coupled_physical_joints, coupled_actuated_axes, m_jointNames, m_COUPLING_actuatedAxesNames, physical_joint_limits); + return true; +} + +bool CouplingICubEye::open(yarp::os::Searchable& config) { + + if(!parseParams(config)) { + yCError(COUPLINGICUBEYE) << "Error parsing parameters"; + return false; + } + + if(!populateCouplingParameters()) { + yCError(COUPLINGICUBEYE) << "Error populating coupling parameters"; + return false; + } + + yCDebug(COUPLINGICUBEYE) << "Opening couplingICubEye" << config.toString(); + return true; +} + +bool CouplingICubEye::convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) { + size_t nrOfPhysicalJoints; + size_t nrOfActuatedAxes; + auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); + ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); + if (!ok || physJointsPos.size() != nrOfPhysicalJoints || actAxesPos.size() != nrOfActuatedAxes) { + yCError(COUPLINGICUBEYE) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size"; + return false; + } + + // eyes_tilt + actAxesPos[0] = physJointsPos[0]; + // eyes_version + actAxesPos[1] = (physJointsPos[1] + physJointsPos[2])/2; + // eyes_vergence + actAxesPos[2] = (physJointsPos[1] - physJointsPos[2]); + + return true; +} + +bool CouplingICubEye::convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) { + size_t nrOfPhysicalJoints; + size_t nrOfActuatedAxes; + auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); + ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); + if (!ok || physJointsVel.size() != nrOfPhysicalJoints || actAxesVel.size() != nrOfActuatedAxes) { + yCError(COUPLINGICUBEYE) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size"; + return false; + } + // eyes_tilt + actAxesVel[0] = physJointsVel[0]; + // eyes_version + actAxesVel[1] = (physJointsVel[1] + physJointsVel[2])/2; + // eyes_vergence + actAxesVel[2] = (physJointsVel[1] - physJointsVel[2]); + + return true; +} + +bool CouplingICubEye::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) { + size_t nrOfPhysicalJoints; + size_t nrOfActuatedAxes; + auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); + ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); + if (!ok || physJointsAcc.size() != nrOfPhysicalJoints || actAxesAcc.size() != nrOfActuatedAxes) { + yCError(COUPLINGICUBEYE) << "convertFromPhysicalJointsToActuatedAxesAcc: input or output vectors have wrong size"; + return false; + } + + // eyes_tilt + actAxesAcc[0] = physJointsAcc[0]; + // eyes_version + actAxesAcc[1] = (physJointsAcc[1] + physJointsAcc[2])/2; + // eyes_vergence + actAxesAcc[2] = (physJointsAcc[1] - physJointsAcc[2]); + + return true; +} + +bool CouplingICubEye::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) { + return false; +} + +bool CouplingICubEye::convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) { + size_t nrOfPhysicalJoints; + size_t nrOfActuatedAxes; + auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); + ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); + if (!ok || physJointsPos.size() != nrOfPhysicalJoints || actAxesPos.size() != nrOfActuatedAxes) { + yCError(COUPLINGICUBEYE) << "convertFromActuatedAxesToPhysicalJointsPos: input or output vectors have wrong size"; + return false; + } + + // eyes_tilt + physJointsPos[0] = actAxesPos[0]; + // l_eye_pan_joint + physJointsPos[1] = actAxesPos[1] + actAxesPos[2]/2; + // r_eye_pan_joint + physJointsPos[2] = actAxesPos[1] - actAxesPos[2]/2; + + return true; +} + +bool CouplingICubEye::convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) { + size_t nrOfPhysicalJoints; + size_t nrOfActuatedAxes; + auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); + ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); + if (!ok || actAxesPos.size() != nrOfActuatedAxes || physJointsVel.size() != nrOfPhysicalJoints || actAxesVel.size() != nrOfActuatedAxes) { + yCError(COUPLINGICUBEYE) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size"; + return false; + } + + // eyes_tilt + physJointsVel[0] = actAxesVel[0]; + // l_eye_pan_joint + physJointsVel[1] = actAxesVel[1] + actAxesVel[2]/2; + // r_eye_pan_joint + physJointsVel[2] = actAxesVel[1] - actAxesVel[2]/2; + + return true; +} + +bool CouplingICubEye::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) { + size_t nrOfPhysicalJoints; + size_t nrOfActuatedAxes; + auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints); + ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes); + if (!ok || actAxesPos.size() != nrOfActuatedAxes || physJointsAcc.size() != nrOfPhysicalJoints || actAxesAcc.size() != nrOfActuatedAxes) { + yCError(COUPLINGICUBEYE) << "convertFromPhysicalJointsToActuatedAxesAcc: input or output vectors have wrong size"; + return false; + } + + // eyes_tilt + physJointsAcc[0] = actAxesAcc[0]; + // l_eye_pan_joint + physJointsAcc[1] = actAxesAcc[1] + actAxesAcc[2]/2; + // r_eye_pan_joint + physJointsAcc[2] = actAxesAcc[1] - actAxesAcc[2]/2; + + return true; +} + +bool CouplingICubEye::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) { + return false; +} diff --git a/src/libraries/icubmod/couplingICubEye/CouplingICubEye.h b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.h new file mode 100644 index 0000000000..965275c9d4 --- /dev/null +++ b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2006-2024 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * BSD-3-Clause license. See the accompanying LICENSE file for details. + */ + +#ifndef COUPLINGICUBEYE_H +#define COUPLINGICUBEYE_H + +#include +#include +#include +#include "CouplingICubEye_ParamsParser.h" + +#include +#include +#include + + +YARP_DECLARE_LOG_COMPONENT(COUPLINGICUBEYE) + +/** + * Coupling law from https://icub-tech-iit.github.io/documentation/icub_kinematics/icub-vergence-version/icub-vergence-version/ + */ + +class CouplingICubEye : public yarp::dev::DeviceDriver, + public yarp::dev::ImplementJointCoupling, + public CouplingICubEye_ParamsParser { +public: + CouplingICubEye() = default; + virtual ~CouplingICubEye() override = default; + bool convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) override; + bool convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) override; + bool convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) override; + bool convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) override; + bool convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) override; + bool convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) override; + bool convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) override; + bool convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) override; + + // //DeviceDriver + /** + * Configure with a set of options. + * @param config The options to use + * @return true iff the object could be configured. + */ + bool open(yarp::os::Searchable& config) override; +private: + + bool populateCouplingParameters(); +}; + +#endif // COUPLINGICUBEYE_H diff --git a/src/libraries/icubmod/couplingICubEye/CouplingICubEye_ParamsParser.cpp b/src/libraries/icubmod/couplingICubEye/CouplingICubEye_ParamsParser.cpp new file mode 100644 index 0000000000..3c15fb9b1a --- /dev/null +++ b/src/libraries/icubmod/couplingICubEye/CouplingICubEye_ParamsParser.cpp @@ -0,0 +1,315 @@ +/* + * SPDX-FileCopyrightText: 2023-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: LGPL-2.1-or-later + */ + + +// Generated by yarpDeviceParamParserGenerator (1.0) +// This is an automatically generated file. Please do not edit it. +// It will be re-generated if the cmake flag ALLOW_DEVICE_PARAM_PARSER_GERNERATION is ON. + +// Generated on: Wed Dec 4 09:08:59 2024 + + +#include "CouplingICubEye_ParamsParser.h" +#include +#include + +namespace { + YARP_LOG_COMPONENT(CouplingICubEyeParamsCOMPONENT, "yarp.device.CouplingICubEye") +} + + +CouplingICubEye_ParamsParser::CouplingICubEye_ParamsParser() +{ +} + + +std::vector CouplingICubEye_ParamsParser::getListOfParams() const +{ + std::vector params; + params.push_back("jointNames"); + params.push_back("LIMITS::jntPosMin"); + params.push_back("LIMITS::jntPosMax"); + params.push_back("COUPLING::device"); + params.push_back("COUPLING::actuatedAxesNames"); + params.push_back("COUPLING::actuatedAxesPosMin"); + params.push_back("COUPLING::actuatedAxesPosMax"); + return params; +} + + +bool CouplingICubEye_ParamsParser::parseParams(const yarp::os::Searchable & config) +{ + //Check for --help option + if (config.check("help")) + { + yCInfo(CouplingICubEyeParamsCOMPONENT) << getDocumentationOfDeviceParams(); + } + + std::string config_string = config.toString(); + yarp::os::Property prop_check(config_string.c_str()); + //Parser of parameter jointNames + { + if (config.check("jointNames")) + { + { + m_jointNames.clear(); + yarp::os::Bottle* tempBot = config.find("jointNames").asList(); + if (tempBot) + { + std::string tempBots = tempBot->toString(); + for (size_t i=0; isize(); i++) + { + m_jointNames.push_back(tempBot->get(i).asString()); + } + } + else + { + yCError(CouplingICubEyeParamsCOMPONENT) <<"parameter 'jointNames' is not a properly formatted bottle"; + } + } + yCInfo(CouplingICubEyeParamsCOMPONENT) << "Parameter 'jointNames' using value:" << m_jointNames; + } + else + { + yCError(CouplingICubEyeParamsCOMPONENT) << "Mandatory parameter 'jointNames' not found!"; + yCError(CouplingICubEyeParamsCOMPONENT) << "Description of the parameter: Names of the physical joints"; + return false; + } + prop_check.unput("jointNames"); + } + + //Parser of parameter LIMITS::jntPosMin + { + yarp::os::Bottle sectionp; + sectionp = config.findGroup("LIMITS"); + if (sectionp.check("jntPosMin")) + { + { + m_LIMITS_jntPosMin.clear(); + yarp::os::Bottle* tempBot = sectionp.find("jntPosMin").asList(); + if (tempBot) + { + std::string tempBots = tempBot->toString(); + for (size_t i=0; isize(); i++) + { + m_LIMITS_jntPosMin.push_back(tempBot->get(i).asFloat64()); + } + } + else + { + yCError(CouplingICubEyeParamsCOMPONENT) <<"parameter 'LIMITS_jntPosMin' is not a properly formatted bottle"; + } + } + yCInfo(CouplingICubEyeParamsCOMPONENT) << "Parameter 'LIMITS::jntPosMin' using value:" << m_LIMITS_jntPosMin; + } + else + { + yCError(CouplingICubEyeParamsCOMPONENT) << "Mandatory parameter 'LIMITS::jntPosMin' not found!"; + yCError(CouplingICubEyeParamsCOMPONENT) << "Description of the parameter: Physical joints' position minimum"; + return false; + } + prop_check.unput("LIMITS::jntPosMin"); + } + + //Parser of parameter LIMITS::jntPosMax + { + yarp::os::Bottle sectionp; + sectionp = config.findGroup("LIMITS"); + if (sectionp.check("jntPosMax")) + { + { + m_LIMITS_jntPosMax.clear(); + yarp::os::Bottle* tempBot = sectionp.find("jntPosMax").asList(); + if (tempBot) + { + std::string tempBots = tempBot->toString(); + for (size_t i=0; isize(); i++) + { + m_LIMITS_jntPosMax.push_back(tempBot->get(i).asFloat64()); + } + } + else + { + yCError(CouplingICubEyeParamsCOMPONENT) <<"parameter 'LIMITS_jntPosMax' is not a properly formatted bottle"; + } + } + yCInfo(CouplingICubEyeParamsCOMPONENT) << "Parameter 'LIMITS::jntPosMax' using value:" << m_LIMITS_jntPosMax; + } + else + { + yCError(CouplingICubEyeParamsCOMPONENT) << "Mandatory parameter 'LIMITS::jntPosMax' not found!"; + yCError(CouplingICubEyeParamsCOMPONENT) << "Description of the parameter: Physical joints' position maximum"; + return false; + } + prop_check.unput("LIMITS::jntPosMax"); + } + + //Parser of parameter COUPLING::device + { + yarp::os::Bottle sectionp; + sectionp = config.findGroup("COUPLING"); + if (sectionp.check("device")) + { + m_COUPLING_device = sectionp.find("device").asString(); + yCInfo(CouplingICubEyeParamsCOMPONENT) << "Parameter 'COUPLING::device' using value:" << m_COUPLING_device; + } + else + { + yCError(CouplingICubEyeParamsCOMPONENT) << "Mandatory parameter 'COUPLING::device' not found!"; + yCError(CouplingICubEyeParamsCOMPONENT) << "Description of the parameter: Name of the device that handles the coupling"; + return false; + } + prop_check.unput("COUPLING::device"); + } + + //Parser of parameter COUPLING::actuatedAxesNames + { + yarp::os::Bottle sectionp; + sectionp = config.findGroup("COUPLING"); + if (sectionp.check("actuatedAxesNames")) + { + { + m_COUPLING_actuatedAxesNames.clear(); + yarp::os::Bottle* tempBot = sectionp.find("actuatedAxesNames").asList(); + if (tempBot) + { + std::string tempBots = tempBot->toString(); + for (size_t i=0; isize(); i++) + { + m_COUPLING_actuatedAxesNames.push_back(tempBot->get(i).asString()); + } + } + else + { + yCError(CouplingICubEyeParamsCOMPONENT) <<"parameter 'COUPLING_actuatedAxesNames' is not a properly formatted bottle"; + } + } + yCInfo(CouplingICubEyeParamsCOMPONENT) << "Parameter 'COUPLING::actuatedAxesNames' using value:" << m_COUPLING_actuatedAxesNames; + } + else + { + yCError(CouplingICubEyeParamsCOMPONENT) << "Mandatory parameter 'COUPLING::actuatedAxesNames' not found!"; + yCError(CouplingICubEyeParamsCOMPONENT) << "Description of the parameter: Names of the actuated axes"; + return false; + } + prop_check.unput("COUPLING::actuatedAxesNames"); + } + + //Parser of parameter COUPLING::actuatedAxesPosMin + { + yarp::os::Bottle sectionp; + sectionp = config.findGroup("COUPLING"); + if (sectionp.check("actuatedAxesPosMin")) + { + { + m_COUPLING_actuatedAxesPosMin.clear(); + yarp::os::Bottle* tempBot = sectionp.find("actuatedAxesPosMin").asList(); + if (tempBot) + { + std::string tempBots = tempBot->toString(); + for (size_t i=0; isize(); i++) + { + m_COUPLING_actuatedAxesPosMin.push_back(tempBot->get(i).asFloat64()); + } + } + else + { + yCError(CouplingICubEyeParamsCOMPONENT) <<"parameter 'COUPLING_actuatedAxesPosMin' is not a properly formatted bottle"; + } + } + yCInfo(CouplingICubEyeParamsCOMPONENT) << "Parameter 'COUPLING::actuatedAxesPosMin' using value:" << m_COUPLING_actuatedAxesPosMin; + } + else + { + yCError(CouplingICubEyeParamsCOMPONENT) << "Mandatory parameter 'COUPLING::actuatedAxesPosMin' not found!"; + yCError(CouplingICubEyeParamsCOMPONENT) << "Description of the parameter: Actuated axes' position minimum"; + return false; + } + prop_check.unput("COUPLING::actuatedAxesPosMin"); + } + + //Parser of parameter COUPLING::actuatedAxesPosMax + { + yarp::os::Bottle sectionp; + sectionp = config.findGroup("COUPLING"); + if (sectionp.check("actuatedAxesPosMax")) + { + { + m_COUPLING_actuatedAxesPosMax.clear(); + yarp::os::Bottle* tempBot = sectionp.find("actuatedAxesPosMax").asList(); + if (tempBot) + { + std::string tempBots = tempBot->toString(); + for (size_t i=0; isize(); i++) + { + m_COUPLING_actuatedAxesPosMax.push_back(tempBot->get(i).asFloat64()); + } + } + else + { + yCError(CouplingICubEyeParamsCOMPONENT) <<"parameter 'COUPLING_actuatedAxesPosMax' is not a properly formatted bottle"; + } + } + yCInfo(CouplingICubEyeParamsCOMPONENT) << "Parameter 'COUPLING::actuatedAxesPosMax' using value:" << m_COUPLING_actuatedAxesPosMax; + } + else + { + yCError(CouplingICubEyeParamsCOMPONENT) << "Mandatory parameter 'COUPLING::actuatedAxesPosMax' not found!"; + yCError(CouplingICubEyeParamsCOMPONENT) << "Description of the parameter: Actuated axes' position maximum"; + return false; + } + prop_check.unput("COUPLING::actuatedAxesPosMax"); + } + + /* + //This code check if the user set some parameter which are not check by the parser + //If the parser is set in strict mode, this will generate an error + if (prop_check.size() > 0) + { + bool extra_params_found = false; + for (auto it=prop_check.begin(); it!=prop_check.end(); it++) + { + if (m_parser_is_strict) + { + yCError(CouplingICubEyeParamsCOMPONENT) << "User asking for parameter: "<name <<" which is unknown to this parser!"; + extra_params_found = true; + } + else + { + yCWarning(CouplingICubEyeParamsCOMPONENT) << "User asking for parameter: "<< it->name <<" which is unknown to this parser!"; + } + } + + if (m_parser_is_strict && extra_params_found) + { + return false; + } + } + */ + return true; +} + + +std::string CouplingICubEye_ParamsParser::getDocumentationOfDeviceParams() const +{ + std::string doc; + doc = doc + std::string("\n=============================================\n"); + doc = doc + std::string("This is the help for device: CouplingICubEye\n"); + doc = doc + std::string("\n"); + doc = doc + std::string("This is the list of the parameters accepted by the device:\n"); + doc = doc + std::string("'jointNames': Names of the physical joints\n"); + doc = doc + std::string("'LIMITS::jntPosMin': Physical joints' position minimum\n"); + doc = doc + std::string("'LIMITS::jntPosMax': Physical joints' position maximum\n"); + doc = doc + std::string("'COUPLING::device': Name of the device that handles the coupling\n"); + doc = doc + std::string("'COUPLING::actuatedAxesNames': Names of the actuated axes\n"); + doc = doc + std::string("'COUPLING::actuatedAxesPosMin': Actuated axes' position minimum\n"); + doc = doc + std::string("'COUPLING::actuatedAxesPosMax': Actuated axes' position maximum\n"); + doc = doc + std::string("\n"); + doc = doc + std::string("Here are some examples of invocation command with yarpdev, with all params:\n"); + doc = doc + " yarpdev --device couplingICubEye --jointNames --LIMITS::jntPosMin --LIMITS::jntPosMax --COUPLING::device --COUPLING::actuatedAxesNames --COUPLING::actuatedAxesPosMin --COUPLING::actuatedAxesPosMax \n"; + doc = doc + std::string("Using only mandatory params:\n"); + doc = doc + " yarpdev --device couplingICubEye --jointNames --LIMITS::jntPosMin --LIMITS::jntPosMax --COUPLING::device --COUPLING::actuatedAxesNames --COUPLING::actuatedAxesPosMin --COUPLING::actuatedAxesPosMax \n"; + doc = doc + std::string("=============================================\n\n"); return doc; +} diff --git a/src/libraries/icubmod/couplingICubEye/CouplingICubEye_ParamsParser.h b/src/libraries/icubmod/couplingICubEye/CouplingICubEye_ParamsParser.h new file mode 100644 index 0000000000..1bb6bc0b1a --- /dev/null +++ b/src/libraries/icubmod/couplingICubEye/CouplingICubEye_ParamsParser.h @@ -0,0 +1,87 @@ +/* + * SPDX-FileCopyrightText: 2023-2023 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: LGPL-2.1-or-later + */ + + +// Generated by yarpDeviceParamParserGenerator (1.0) +// This is an automatically generated file. Please do not edit it. +// It will be re-generated if the cmake flag ALLOW_DEVICE_PARAM_PARSER_GERNERATION is ON. + +// Generated on: Wed Dec 4 09:08:59 2024 + + +#ifndef COUPLINGICUBEYE_PARAMSPARSER_H +#define COUPLINGICUBEYE_PARAMSPARSER_H + +#include +#include +#include +#include + +/** +* This class is the parameters parser for class CouplingICubEye. +* +* These are the used parameters: +* | Group name | Parameter name | Type | Units | Default Value | Required | Description | Notes | +* |:----------:|:------------------:|:--------------:|:-----:|:-------------:|:--------:|:--------------------------------------------:|:-----:| +* | - | jointNames | vector | - | - | 1 | Names of the physical joints | - | +* | LIMITS | jntPosMin | vector | - | - | 1 | Physical joints' position minimum | - | +* | LIMITS | jntPosMax | vector | - | - | 1 | Physical joints' position maximum | - | +* | COUPLING | device | string | - | - | 1 | Name of the device that handles the coupling | - | +* | COUPLING | actuatedAxesNames | vector | - | - | 1 | Names of the actuated axes | - | +* | COUPLING | actuatedAxesPosMin | vector | - | - | 1 | Actuated axes' position minimum | - | +* | COUPLING | actuatedAxesPosMax | vector | - | - | 1 | Actuated axes' position maximum | - | +* +* The device can be launched by yarpdev using one of the following examples (with and without all optional parameters): +* \code{.unparsed} +* yarpdev --device couplingICubEye --jointNames --LIMITS::jntPosMin --LIMITS::jntPosMax --COUPLING::device --COUPLING::actuatedAxesNames --COUPLING::actuatedAxesPosMin --COUPLING::actuatedAxesPosMax +* \endcode +* +* \code{.unparsed} +* yarpdev --device couplingICubEye --jointNames --LIMITS::jntPosMin --LIMITS::jntPosMax --COUPLING::device --COUPLING::actuatedAxesNames --COUPLING::actuatedAxesPosMin --COUPLING::actuatedAxesPosMax +* \endcode +* +*/ + +class CouplingICubEye_ParamsParser : public yarp::dev::IDeviceDriverParams +{ +public: + CouplingICubEye_ParamsParser(); + ~CouplingICubEye_ParamsParser() override = default; + +public: + const std::string m_device_classname = {"CouplingICubEye"}; + const std::string m_device_name = {"couplingICubEye"}; + bool m_parser_is_strict = false; + struct parser_version_type + { + int major = 1; + int minor = 0; + }; + const parser_version_type m_parser_version = {}; + + const std::string m_jointNames_defaultValue = {""}; + const std::string m_LIMITS_jntPosMin_defaultValue = {""}; + const std::string m_LIMITS_jntPosMax_defaultValue = {""}; + const std::string m_COUPLING_device_defaultValue = {""}; + const std::string m_COUPLING_actuatedAxesNames_defaultValue = {""}; + const std::string m_COUPLING_actuatedAxesPosMin_defaultValue = {""}; + const std::string m_COUPLING_actuatedAxesPosMax_defaultValue = {""}; + + std::vector m_jointNames = {}; //This default value is autogenerated. It is highly recommended to provide a suggested value also for mandatory parameters. + std::vector m_LIMITS_jntPosMin = {}; //This default value is autogenerated. It is highly recommended to provide a suggested value also for mandatory parameters. + std::vector m_LIMITS_jntPosMax = {}; //This default value is autogenerated. It is highly recommended to provide a suggested value also for mandatory parameters. + std::string m_COUPLING_device = {}; //This default value is autogenerated. It is highly recommended to provide a suggested value also for mandatory parameters. + std::vector m_COUPLING_actuatedAxesNames = {}; //This default value is autogenerated. It is highly recommended to provide a suggested value also for mandatory parameters. + std::vector m_COUPLING_actuatedAxesPosMin = {}; //This default value is autogenerated. It is highly recommended to provide a suggested value also for mandatory parameters. + std::vector m_COUPLING_actuatedAxesPosMax = {}; //This default value is autogenerated. It is highly recommended to provide a suggested value also for mandatory parameters. + + bool parseParams(const yarp::os::Searchable & config) override; + std::string getDeviceClassName() const override { return m_device_classname; } + std::string getDeviceName() const override { return m_device_name; } + std::string getDocumentationOfDeviceParams() const override; + std::vector getListOfParams() const override; +}; + +#endif diff --git a/src/libraries/icubmod/couplingICubEye/CouplingICubEye_params.md b/src/libraries/icubmod/couplingICubEye/CouplingICubEye_params.md new file mode 100644 index 0000000000..cbb09dd7cf --- /dev/null +++ b/src/libraries/icubmod/couplingICubEye/CouplingICubEye_params.md @@ -0,0 +1,9 @@ +| Group name | Parameter name | Type | Units | Default Value | Required | Description | Notes | +|:---------------:|:------------------:|:---------------:|:-------:|:--------------:|:--------:|:----------------------------------------------:|:--------------------------------:| +| | jointNames | vector | - | - | Yes | Names of the physical joints | | +| LIMITS | jntPosMin | vector | - | - | Yes | Physical joints' position minimum | | +| LIMITS | jntPosMax | vector | - | - | Yes | Physical joints' position maximum | | +| COUPLING | device | string | - | - | Yes | Name of the device that handles the coupling | | +| COUPLING | actuatedAxesNames | vector | - | - | Yes | Names of the actuated axes | | +| COUPLING | actuatedAxesPosMin | vector | - | - | Yes | Actuated axes' position minimum | | +| COUPLING | actuatedAxesPosMax | vector | - | - | Yes | Actuated axes' position maximum | | \ No newline at end of file diff --git a/src/libraries/icubmod/couplingICubEye/README.md b/src/libraries/icubmod/couplingICubEye/README.md new file mode 100644 index 0000000000..dc28dcb484 --- /dev/null +++ b/src/libraries/icubmod/couplingICubEye/README.md @@ -0,0 +1,48 @@ +![YARP logo](https://raw.githubusercontent.com/robotology/yarp/master/doc/images/yarp-robot-24.png "couplingICubEye") +## couplingICubEye + + +This is the device for [YARP](https://www.yarp.it/) for handling the coupling of the [eyes](https://icub-tech-iit.github.io/documentation/icub_kinematics/icub-vergence-version/icub-vergence-version/) + +## Device documentation + +This device driver exposes the `yarp::dev::IJointCoupling` interface to getting +quantities from the physical joints to the actuated axes domain and viceversa. +See the documentation for more details about each interface. + + +| YARP device name | +|:----------------:| +| `couplingICubEye` | + +Parameters used by this device are: + +| Parameter name | SubParameter | Type | Read / write | Units | Default Value | Required | Description | Notes | +|:----------------------------:|:-----------------:|:--------------:|:------------:|:-------:|:-------------:|:---------------:|:-------------------------------------------------------------------------------------:|:---------------------------------------------------------------------:| +| `jointNames` | - | list of strings | Read / write | - | - | Yes | Names of the physical joints | | +| `LIMITS` | - | group | Read / write | - | - | Yes | group containing the physical joint limits | | +| | `jntPosMin` | list of double | Read / write | - | - | Yes | Physical joints' position minimum | | +| | `jntPosMax` | list of double | Read / write | - | - | Yes | Physical joints' position maximum | | +| `COUPLING` | - | group | Read / write | - | - | Yes | The group containing the coupling description +| | `actuatedAxesNames` | list of strings | Read / write | - | - | Yes | Names of the actuated axes | | +| | `actuatedAxesPosMin` | list of strings | Read / write | - | - | Yes | Actuated axes' position minimum | | +| | `actuatedAxesPosMax` | list of strings | Read / write | - | - | Yes | Actuated axes' position maximum | | + + +Configuration file using `.ini` format: + +```ini +device couplingICubEye +jointNames eyes_tilt l_eye_pan_joint r_eye_pan_joint + +[COUPLING] +device couplingICubEye +actuatedAxesNames eyes_tilt eyes_version eyes_vergence +actuatedAxesPosMin -30.0 -30.0 0.0 +actuatedAxesPosMax 30.0 30.0 50.0 + +[LIMITS] +jntPosMax 30.0 55.0 30.0 +jntPosMin -30.0 -30.0 -55.0 +jntVelMax 100.0 100.0 100.0 +``` \ No newline at end of file