From d6784def9a3edf8dc80a2a6c7b93b738270f2389 Mon Sep 17 00:00:00 2001 From: Martina Gloria Date: Mon, 2 Dec 2024 15:17:28 +0100 Subject: [PATCH 1/5] Add couplingICubEyeMk3 device --- src/libraries/icubmod/CMakeLists.txt | 1 + .../icubmod/couplingICubEyeMk3/CMakeLists.txt | 42 +++ .../couplingICubEyeMk3/CouplingICubEyeMk3.cpp | 158 +++++++++ .../couplingICubEyeMk3/CouplingICubEyeMk3.h | 56 ++++ .../CouplingICubEyeMk3_ParamsParser.cpp | 315 ++++++++++++++++++ .../CouplingICubEyeMk3_ParamsParser.h | 87 +++++ .../CouplingICubEyeMk3_params.md | 9 + .../icubmod/couplingICubEyeMk3/README.md | 48 +++ 8 files changed, 716 insertions(+) create mode 100644 src/libraries/icubmod/couplingICubEyeMk3/CMakeLists.txt create mode 100644 src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.cpp create mode 100644 src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.h create mode 100644 src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_ParamsParser.cpp create mode 100644 src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_ParamsParser.h create mode 100644 src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_params.md create mode 100644 src/libraries/icubmod/couplingICubEyeMk3/README.md diff --git a/src/libraries/icubmod/CMakeLists.txt b/src/libraries/icubmod/CMakeLists.txt index a2e3169344..2b47ec9c01 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(couplingICubEyeMk3) if (ICUB_ICUBINTERFACE_EXPERIMENTAL) add_subdirectory(imu3DM_GX3) endif() diff --git a/src/libraries/icubmod/couplingICubEyeMk3/CMakeLists.txt b/src/libraries/icubmod/couplingICubEyeMk3/CMakeLists.txt new file mode 100644 index 0000000000..93499cc203 --- /dev/null +++ b/src/libraries/icubmod/couplingICubEyeMk3/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(couplingICubEyeMk3 + CATEGORY device + TYPE CouplingICubEyeMk3 + INCLUDE CouplingICubEyeMk3.h + GENERATE_PARSER + DEFAULT ON) + +if(ENABLE_couplingICubEyeMk3) + option(ALLOW_DEVICE_PARAM_PARSER_GENERATION "Generate the param parser for couplingICubEyeMk3 device" OFF) + yarp_add_plugin(yarp_couplingICubEyeMk3) + + if(MSVC) + add_definitions(-D_USE_MATH_DEFINES) + endif() + + target_sources(yarp_couplingICubEyeMk3 PRIVATE CouplingICubEyeMk3.cpp + CouplingICubEyeMk3.h + CouplingICubEyeMk3_ParamsParser.cpp + CouplingICubEyeMk3_ParamsParser.h) + + target_link_libraries(yarp_couplingICubEyeMk3 PRIVATE YARP::YARP_os + YARP::YARP_dev) + list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS YARP_os + YARP_dev) + + yarp_install(TARGETS yarp_couplingICubEyeMk3 + 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_couplingICubEyeMk3 PROPERTY FOLDER "Plugins/Device") +endif() \ No newline at end of file diff --git a/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.cpp b/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.cpp new file mode 100644 index 0000000000..db4dfd617b --- /dev/null +++ b/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.cpp @@ -0,0 +1,158 @@ +/* + * 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 "CouplingICubEyeMk3.h" +#include +#include +#include +#include + + +YARP_LOG_COMPONENT(COUPLINGICUBEYEMK3, "yarp.device.couplingICubEyeMk3") + + +bool CouplingICubEyeMk3::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 CouplingICubEyeMk3::open(yarp::os::Searchable& config) { + + if(!parseParams(config)) { + yCError(COUPLINGICUBEYEMK3) << "Error parsing parameters"; + return false; + } + + yCDebug(COUPLINGICUBEYEMK3) << "Opening couplingICubEyeMk3" << config.toString(); + return true; +} + +bool CouplingICubEyeMk3::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(COUPLINGICUBEYEMK3) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size"; + return false; + } + + //version + actAxesPos[1] = (physJointsPos[1] + physJointsPos[2])/2; + //vergence + actAxesPos[2] = (physJointsPos[1] - physJointsPos[2]); + + return true; +} + +bool CouplingICubEyeMk3::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(COUPLINGICUBEYEMK3) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size"; + return false; + } + + //version + actAxesPos[1] = (physJointsPos[1] + physJointsPos[2])/2; + //vergence + actAxesPos[2] = (physJointsPos[1] - physJointsPos[2]); + + return true; +} + +bool CouplingICubEyeMk3::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(COUPLINGICUBEYEMK3) << "convertFromPhysicalJointsToActuatedAxesAcc: input or output vectors have wrong size"; + return false; + } + + //version + actAxesPos[1] = (physJointsPos[1] + physJointsPos[2])/2; + //vergence + actAxesPos[2] = (physJointsPos[1] - physJointsPos[2]); + + return true; +} + +bool CouplingICubEyeMk3::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) { + return false; +} + +bool CouplingICubEyeMk3::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(COUPLINGICUBEYEMK3) << "convertFromActuatedAxesToPhysicalJointsPos: input or output vectors have wrong size"; + return false; + } + + // 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 CouplingICubEyeMk3::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(COUPLINGICUBEYEMK3) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size"; + return false; + } + + // 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 CouplingICubEyeMk3::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(COUPLINGICUBEYEMK3) << "convertFromPhysicalJointsToActuatedAxesAcc: input or output vectors have wrong size"; + return false; + } + + // 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 CouplingICubEyeMk3::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) { + return false; +} \ No newline at end of file diff --git a/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.h b/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.h new file mode 100644 index 0000000000..d5ada7c474 --- /dev/null +++ b/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.h @@ -0,0 +1,56 @@ +/* + * 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 COUPLINGICUBEYEMK3_H +#define COUPLINGICUBEYEMK3_H + +#include +#include +#include +#include "CouplingICubEyeMk3_ParamsParser.h" + +#include +#include +#include + + +YARP_DECLARE_LOG_COMPONENT(COUPLINGICUBEYEMK3) + +/** + * Coupling law from https://icub-tech-iit.github.io/documentation/icub_kinematics/icub-vergence-version/icub-vergence-version/ + */ +/** TBD + */ +class CouplingICubEyeMk3 : public yarp::dev::DeviceDriver, + public yarp::dev::ImplementJointCoupling, + public CouplingICubEyeMk3_ParamsParser { +public: + CouplingICubEyeMk3() = default; + virtual ~CouplingICubEyeMk3() 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 // COUPLINGICUBEYEMK3_H \ No newline at end of file diff --git a/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_ParamsParser.cpp b/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_ParamsParser.cpp new file mode 100644 index 0000000000..b247cf4d2f --- /dev/null +++ b/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_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: Mon Dec 2 15:29:09 2024 + + +#include "CouplingICubEyeMk3_ParamsParser.h" +#include +#include + +namespace { + YARP_LOG_COMPONENT(CouplingICubEyeMk3ParamsCOMPONENT, "yarp.device.CouplingICubEyeMk3") +} + + +CouplingICubEyeMk3_ParamsParser::CouplingICubEyeMk3_ParamsParser() +{ +} + + +std::vector CouplingICubEyeMk3_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 CouplingICubEyeMk3_ParamsParser::parseParams(const yarp::os::Searchable & config) +{ + //Check for --help option + if (config.check("help")) + { + yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << 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(CouplingICubEyeMk3ParamsCOMPONENT) <<"parameter 'jointNames' is not a properly formatted bottle"; + } + } + yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << "Parameter 'jointNames' using value:" << m_jointNames; + } + else + { + yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Mandatory parameter 'jointNames' not found!"; + yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "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(CouplingICubEyeMk3ParamsCOMPONENT) <<"parameter 'LIMITS_jntPosMin' is not a properly formatted bottle"; + } + } + yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << "Parameter 'LIMITS::jntPosMin' using value:" << m_LIMITS_jntPosMin; + } + else + { + yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Mandatory parameter 'LIMITS::jntPosMin' not found!"; + yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "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(CouplingICubEyeMk3ParamsCOMPONENT) <<"parameter 'LIMITS_jntPosMax' is not a properly formatted bottle"; + } + } + yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << "Parameter 'LIMITS::jntPosMax' using value:" << m_LIMITS_jntPosMax; + } + else + { + yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Mandatory parameter 'LIMITS::jntPosMax' not found!"; + yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "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(CouplingICubEyeMk3ParamsCOMPONENT) << "Parameter 'COUPLING::device' using value:" << m_COUPLING_device; + } + else + { + yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Mandatory parameter 'COUPLING::device' not found!"; + yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "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(CouplingICubEyeMk3ParamsCOMPONENT) <<"parameter 'COUPLING_actuatedAxesNames' is not a properly formatted bottle"; + } + } + yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << "Parameter 'COUPLING::actuatedAxesNames' using value:" << m_COUPLING_actuatedAxesNames; + } + else + { + yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Mandatory parameter 'COUPLING::actuatedAxesNames' not found!"; + yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "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(CouplingICubEyeMk3ParamsCOMPONENT) <<"parameter 'COUPLING_actuatedAxesPosMin' is not a properly formatted bottle"; + } + } + yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << "Parameter 'COUPLING::actuatedAxesPosMin' using value:" << m_COUPLING_actuatedAxesPosMin; + } + else + { + yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Mandatory parameter 'COUPLING::actuatedAxesPosMin' not found!"; + yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "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(CouplingICubEyeMk3ParamsCOMPONENT) <<"parameter 'COUPLING_actuatedAxesPosMax' is not a properly formatted bottle"; + } + } + yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << "Parameter 'COUPLING::actuatedAxesPosMax' using value:" << m_COUPLING_actuatedAxesPosMax; + } + else + { + yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Mandatory parameter 'COUPLING::actuatedAxesPosMax' not found!"; + yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "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(CouplingICubEyeMk3ParamsCOMPONENT) << "User asking for parameter: "<name <<" which is unknown to this parser!"; + extra_params_found = true; + } + else + { + yCWarning(CouplingICubEyeMk3ParamsCOMPONENT) << "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 CouplingICubEyeMk3_ParamsParser::getDocumentationOfDeviceParams() const +{ + std::string doc; + doc = doc + std::string("\n=============================================\n"); + doc = doc + std::string("This is the help for device: CouplingICubEyeMk3\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 couplingICubEyeMk3 --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 couplingICubEyeMk3 --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/couplingICubEyeMk3/CouplingICubEyeMk3_ParamsParser.h b/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_ParamsParser.h new file mode 100644 index 0000000000..3e9b27561e --- /dev/null +++ b/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_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: Mon Dec 2 15:29:09 2024 + + +#ifndef COUPLINGICUBEYEMK3_PARAMSPARSER_H +#define COUPLINGICUBEYEMK3_PARAMSPARSER_H + +#include +#include +#include +#include + +/** +* This class is the parameters parser for class CouplingICubEyeMk3. +* +* 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 couplingICubEyeMk3 --jointNames --LIMITS::jntPosMin --LIMITS::jntPosMax --COUPLING::device --COUPLING::actuatedAxesNames --COUPLING::actuatedAxesPosMin --COUPLING::actuatedAxesPosMax +* \endcode +* +* \code{.unparsed} +* yarpdev --device couplingICubEyeMk3 --jointNames --LIMITS::jntPosMin --LIMITS::jntPosMax --COUPLING::device --COUPLING::actuatedAxesNames --COUPLING::actuatedAxesPosMin --COUPLING::actuatedAxesPosMax +* \endcode +* +*/ + +class CouplingICubEyeMk3_ParamsParser : public yarp::dev::IDeviceDriverParams +{ +public: + CouplingICubEyeMk3_ParamsParser(); + ~CouplingICubEyeMk3_ParamsParser() override = default; + +public: + const std::string m_device_classname = {"CouplingICubEyeMk3"}; + const std::string m_device_name = {"couplingICubEyeMk3"}; + 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/couplingICubEyeMk3/CouplingICubEyeMk3_params.md b/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_params.md new file mode 100644 index 0000000000..cbb09dd7cf --- /dev/null +++ b/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_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/couplingICubEyeMk3/README.md b/src/libraries/icubmod/couplingICubEyeMk3/README.md new file mode 100644 index 0000000000..05cbbce798 --- /dev/null +++ b/src/libraries/icubmod/couplingICubEyeMk3/README.md @@ -0,0 +1,48 @@ +![YARP logo](https://raw.githubusercontent.com/robotology/yarp/master/doc/images/yarp-robot-24.png "couplingICubEyeMk3") +## couplingICubEyeMk3 + + +This is the device for [YARP](https://www.yarp.it/) for handling the coupling of the [eyes mk3](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 | +|:----------------:| +| `couplingICubEyeMk3` | + +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 couplingICubEyeMk3 +jointNames eyes_tilt l_eye_pan_joint r_eye_pan_joint + +[COUPLING] +device couplingICubEyeMk3 +actuatedAxesNames eyes_version eyes_vergence +actuatedAxesPosMin -30.0 0.0 +actuatedAxesPosMax 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 From f24e603c45b23b6c5e636512c490e48690071a5e Mon Sep 17 00:00:00 2001 From: Martina Gloria Date: Wed, 4 Dec 2024 09:09:24 +0100 Subject: [PATCH 2/5] Change device name to couplingICubEye --- src/libraries/icubmod/CMakeLists.txt | 2 +- .../CMakeLists.txt | 26 +-- .../couplingICubEye/CouplingICubEye.cpp | 169 ++++++++++++++++++ .../CouplingICubEye.h} | 18 +- .../CouplingICubEye_ParamsParser.cpp} | 80 ++++----- .../CouplingICubEye_ParamsParser.h} | 22 +-- .../CouplingICubEye_params.md} | 0 .../README.md | 12 +- .../couplingICubEyeMk3/CouplingICubEyeMk3.cpp | 158 ---------------- 9 files changed, 249 insertions(+), 238 deletions(-) rename src/libraries/icubmod/{couplingICubEyeMk3 => couplingICubEye}/CMakeLists.txt (58%) create mode 100644 src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp rename src/libraries/icubmod/{couplingICubEyeMk3/CouplingICubEyeMk3.h => couplingICubEye/CouplingICubEye.h} (85%) rename src/libraries/icubmod/{couplingICubEyeMk3/CouplingICubEyeMk3_ParamsParser.cpp => couplingICubEye/CouplingICubEye_ParamsParser.cpp} (62%) rename src/libraries/icubmod/{couplingICubEyeMk3/CouplingICubEyeMk3_ParamsParser.h => couplingICubEye/CouplingICubEye_ParamsParser.h} (78%) rename src/libraries/icubmod/{couplingICubEyeMk3/CouplingICubEyeMk3_params.md => couplingICubEye/CouplingICubEye_params.md} (100%) rename src/libraries/icubmod/{couplingICubEyeMk3 => couplingICubEye}/README.md (90%) delete mode 100644 src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.cpp diff --git a/src/libraries/icubmod/CMakeLists.txt b/src/libraries/icubmod/CMakeLists.txt index 2b47ec9c01..932dde9f0a 100644 --- a/src/libraries/icubmod/CMakeLists.txt +++ b/src/libraries/icubmod/CMakeLists.txt @@ -55,7 +55,7 @@ yarp_begin_plugin_library(icubmod QUIET) add_subdirectory(rawValuesPublisherServer) add_subdirectory(fakeRawValuesPublisher) add_subdirectory(couplingICubHandMk2) - add_subdirectory(couplingICubEyeMk3) + add_subdirectory(couplingICubEye) if (ICUB_ICUBINTERFACE_EXPERIMENTAL) add_subdirectory(imu3DM_GX3) endif() diff --git a/src/libraries/icubmod/couplingICubEyeMk3/CMakeLists.txt b/src/libraries/icubmod/couplingICubEye/CMakeLists.txt similarity index 58% rename from src/libraries/icubmod/couplingICubEyeMk3/CMakeLists.txt rename to src/libraries/icubmod/couplingICubEye/CMakeLists.txt index 93499cc203..e99841fbf9 100644 --- a/src/libraries/icubmod/couplingICubEyeMk3/CMakeLists.txt +++ b/src/libraries/icubmod/couplingICubEye/CMakeLists.txt @@ -4,32 +4,32 @@ # 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(couplingICubEyeMk3 +yarp_prepare_plugin(couplingICubEye CATEGORY device - TYPE CouplingICubEyeMk3 - INCLUDE CouplingICubEyeMk3.h + TYPE CouplingICubEye + INCLUDE CouplingICubEye.h GENERATE_PARSER DEFAULT ON) -if(ENABLE_couplingICubEyeMk3) - option(ALLOW_DEVICE_PARAM_PARSER_GENERATION "Generate the param parser for couplingICubEyeMk3 device" OFF) - yarp_add_plugin(yarp_couplingICubEyeMk3) +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_couplingICubEyeMk3 PRIVATE CouplingICubEyeMk3.cpp - CouplingICubEyeMk3.h - CouplingICubEyeMk3_ParamsParser.cpp - CouplingICubEyeMk3_ParamsParser.h) + target_sources(yarp_couplingICubEye PRIVATE CouplingICubEye.cpp + CouplingICubEye.h + CouplingICubEye_ParamsParser.cpp + CouplingICubEye_ParamsParser.h) - target_link_libraries(yarp_couplingICubEyeMk3 PRIVATE YARP::YARP_os + 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_couplingICubEyeMk3 + yarp_install(TARGETS yarp_couplingICubEye EXPORT YARP_${YARP_PLUGIN_MASTER} COMPONENT ${YARP_PLUGIN_MASTER} LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} @@ -38,5 +38,5 @@ if(ENABLE_couplingICubEyeMk3) set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE) - set_property(TARGET yarp_couplingICubEyeMk3 PROPERTY FOLDER "Plugins/Device") + 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..d901434376 --- /dev/null +++ b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp @@ -0,0 +1,169 @@ +/* + * 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; + } + + 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; +} \ No newline at end of file diff --git a/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.h b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.h similarity index 85% rename from src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.h rename to src/libraries/icubmod/couplingICubEye/CouplingICubEye.h index d5ada7c474..e433d7a28d 100644 --- a/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.h +++ b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.h @@ -6,32 +6,32 @@ * BSD-3-Clause license. See the accompanying LICENSE file for details. */ -#ifndef COUPLINGICUBEYEMK3_H -#define COUPLINGICUBEYEMK3_H +#ifndef COUPLINGICUBEYE_H +#define COUPLINGICUBEYE_H #include #include #include -#include "CouplingICubEyeMk3_ParamsParser.h" +#include "CouplingICubEye_ParamsParser.h" #include #include #include -YARP_DECLARE_LOG_COMPONENT(COUPLINGICUBEYEMK3) +YARP_DECLARE_LOG_COMPONENT(COUPLINGICUBEYE) /** * Coupling law from https://icub-tech-iit.github.io/documentation/icub_kinematics/icub-vergence-version/icub-vergence-version/ */ /** TBD */ -class CouplingICubEyeMk3 : public yarp::dev::DeviceDriver, +class CouplingICubEye : public yarp::dev::DeviceDriver, public yarp::dev::ImplementJointCoupling, - public CouplingICubEyeMk3_ParamsParser { + public CouplingICubEye_ParamsParser { public: - CouplingICubEyeMk3() = default; - virtual ~CouplingICubEyeMk3() override = default; + 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; @@ -53,4 +53,4 @@ class CouplingICubEyeMk3 : public yarp::dev::DeviceDriver, bool populateCouplingParameters(); }; -#endif // COUPLINGICUBEYEMK3_H \ No newline at end of file +#endif // COUPLINGICUBEYE_H \ No newline at end of file diff --git a/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_ParamsParser.cpp b/src/libraries/icubmod/couplingICubEye/CouplingICubEye_ParamsParser.cpp similarity index 62% rename from src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_ParamsParser.cpp rename to src/libraries/icubmod/couplingICubEye/CouplingICubEye_ParamsParser.cpp index b247cf4d2f..3c15fb9b1a 100644 --- a/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_ParamsParser.cpp +++ b/src/libraries/icubmod/couplingICubEye/CouplingICubEye_ParamsParser.cpp @@ -8,24 +8,24 @@ // 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: Mon Dec 2 15:29:09 2024 +// Generated on: Wed Dec 4 09:08:59 2024 -#include "CouplingICubEyeMk3_ParamsParser.h" +#include "CouplingICubEye_ParamsParser.h" #include #include namespace { - YARP_LOG_COMPONENT(CouplingICubEyeMk3ParamsCOMPONENT, "yarp.device.CouplingICubEyeMk3") + YARP_LOG_COMPONENT(CouplingICubEyeParamsCOMPONENT, "yarp.device.CouplingICubEye") } -CouplingICubEyeMk3_ParamsParser::CouplingICubEyeMk3_ParamsParser() +CouplingICubEye_ParamsParser::CouplingICubEye_ParamsParser() { } -std::vector CouplingICubEyeMk3_ParamsParser::getListOfParams() const +std::vector CouplingICubEye_ParamsParser::getListOfParams() const { std::vector params; params.push_back("jointNames"); @@ -39,12 +39,12 @@ std::vector CouplingICubEyeMk3_ParamsParser::getListOfParams() cons } -bool CouplingICubEyeMk3_ParamsParser::parseParams(const yarp::os::Searchable & config) +bool CouplingICubEye_ParamsParser::parseParams(const yarp::os::Searchable & config) { //Check for --help option if (config.check("help")) { - yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << getDocumentationOfDeviceParams(); + yCInfo(CouplingICubEyeParamsCOMPONENT) << getDocumentationOfDeviceParams(); } std::string config_string = config.toString(); @@ -66,15 +66,15 @@ bool CouplingICubEyeMk3_ParamsParser::parseParams(const yarp::os::Searchabl } else { - yCError(CouplingICubEyeMk3ParamsCOMPONENT) <<"parameter 'jointNames' is not a properly formatted bottle"; + yCError(CouplingICubEyeParamsCOMPONENT) <<"parameter 'jointNames' is not a properly formatted bottle"; } } - yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << "Parameter 'jointNames' using value:" << m_jointNames; + yCInfo(CouplingICubEyeParamsCOMPONENT) << "Parameter 'jointNames' using value:" << m_jointNames; } else { - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Mandatory parameter 'jointNames' not found!"; - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Description of the parameter: Names of the physical joints"; + yCError(CouplingICubEyeParamsCOMPONENT) << "Mandatory parameter 'jointNames' not found!"; + yCError(CouplingICubEyeParamsCOMPONENT) << "Description of the parameter: Names of the physical joints"; return false; } prop_check.unput("jointNames"); @@ -99,15 +99,15 @@ bool CouplingICubEyeMk3_ParamsParser::parseParams(const yarp::os::Searchabl } else { - yCError(CouplingICubEyeMk3ParamsCOMPONENT) <<"parameter 'LIMITS_jntPosMin' is not a properly formatted bottle"; + yCError(CouplingICubEyeParamsCOMPONENT) <<"parameter 'LIMITS_jntPosMin' is not a properly formatted bottle"; } } - yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << "Parameter 'LIMITS::jntPosMin' using value:" << m_LIMITS_jntPosMin; + yCInfo(CouplingICubEyeParamsCOMPONENT) << "Parameter 'LIMITS::jntPosMin' using value:" << m_LIMITS_jntPosMin; } else { - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Mandatory parameter 'LIMITS::jntPosMin' not found!"; - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Description of the parameter: Physical joints' position minimum"; + 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"); @@ -132,15 +132,15 @@ bool CouplingICubEyeMk3_ParamsParser::parseParams(const yarp::os::Searchabl } else { - yCError(CouplingICubEyeMk3ParamsCOMPONENT) <<"parameter 'LIMITS_jntPosMax' is not a properly formatted bottle"; + yCError(CouplingICubEyeParamsCOMPONENT) <<"parameter 'LIMITS_jntPosMax' is not a properly formatted bottle"; } } - yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << "Parameter 'LIMITS::jntPosMax' using value:" << m_LIMITS_jntPosMax; + yCInfo(CouplingICubEyeParamsCOMPONENT) << "Parameter 'LIMITS::jntPosMax' using value:" << m_LIMITS_jntPosMax; } else { - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Mandatory parameter 'LIMITS::jntPosMax' not found!"; - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Description of the parameter: Physical joints' position maximum"; + 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"); @@ -153,12 +153,12 @@ bool CouplingICubEyeMk3_ParamsParser::parseParams(const yarp::os::Searchabl if (sectionp.check("device")) { m_COUPLING_device = sectionp.find("device").asString(); - yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << "Parameter 'COUPLING::device' using value:" << m_COUPLING_device; + yCInfo(CouplingICubEyeParamsCOMPONENT) << "Parameter 'COUPLING::device' using value:" << m_COUPLING_device; } else { - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Mandatory parameter 'COUPLING::device' not found!"; - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Description of the parameter: Name of the device that handles the coupling"; + 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"); @@ -183,15 +183,15 @@ bool CouplingICubEyeMk3_ParamsParser::parseParams(const yarp::os::Searchabl } else { - yCError(CouplingICubEyeMk3ParamsCOMPONENT) <<"parameter 'COUPLING_actuatedAxesNames' is not a properly formatted bottle"; + yCError(CouplingICubEyeParamsCOMPONENT) <<"parameter 'COUPLING_actuatedAxesNames' is not a properly formatted bottle"; } } - yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << "Parameter 'COUPLING::actuatedAxesNames' using value:" << m_COUPLING_actuatedAxesNames; + yCInfo(CouplingICubEyeParamsCOMPONENT) << "Parameter 'COUPLING::actuatedAxesNames' using value:" << m_COUPLING_actuatedAxesNames; } else { - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Mandatory parameter 'COUPLING::actuatedAxesNames' not found!"; - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Description of the parameter: Names of the actuated axes"; + 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"); @@ -216,15 +216,15 @@ bool CouplingICubEyeMk3_ParamsParser::parseParams(const yarp::os::Searchabl } else { - yCError(CouplingICubEyeMk3ParamsCOMPONENT) <<"parameter 'COUPLING_actuatedAxesPosMin' is not a properly formatted bottle"; + yCError(CouplingICubEyeParamsCOMPONENT) <<"parameter 'COUPLING_actuatedAxesPosMin' is not a properly formatted bottle"; } } - yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << "Parameter 'COUPLING::actuatedAxesPosMin' using value:" << m_COUPLING_actuatedAxesPosMin; + yCInfo(CouplingICubEyeParamsCOMPONENT) << "Parameter 'COUPLING::actuatedAxesPosMin' using value:" << m_COUPLING_actuatedAxesPosMin; } else { - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Mandatory parameter 'COUPLING::actuatedAxesPosMin' not found!"; - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Description of the parameter: Actuated axes' position minimum"; + 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"); @@ -249,15 +249,15 @@ bool CouplingICubEyeMk3_ParamsParser::parseParams(const yarp::os::Searchabl } else { - yCError(CouplingICubEyeMk3ParamsCOMPONENT) <<"parameter 'COUPLING_actuatedAxesPosMax' is not a properly formatted bottle"; + yCError(CouplingICubEyeParamsCOMPONENT) <<"parameter 'COUPLING_actuatedAxesPosMax' is not a properly formatted bottle"; } } - yCInfo(CouplingICubEyeMk3ParamsCOMPONENT) << "Parameter 'COUPLING::actuatedAxesPosMax' using value:" << m_COUPLING_actuatedAxesPosMax; + yCInfo(CouplingICubEyeParamsCOMPONENT) << "Parameter 'COUPLING::actuatedAxesPosMax' using value:" << m_COUPLING_actuatedAxesPosMax; } else { - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Mandatory parameter 'COUPLING::actuatedAxesPosMax' not found!"; - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "Description of the parameter: Actuated axes' position maximum"; + 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"); @@ -273,12 +273,12 @@ bool CouplingICubEyeMk3_ParamsParser::parseParams(const yarp::os::Searchabl { if (m_parser_is_strict) { - yCError(CouplingICubEyeMk3ParamsCOMPONENT) << "User asking for parameter: "<name <<" which is unknown to this parser!"; + yCError(CouplingICubEyeParamsCOMPONENT) << "User asking for parameter: "<name <<" which is unknown to this parser!"; extra_params_found = true; } else { - yCWarning(CouplingICubEyeMk3ParamsCOMPONENT) << "User asking for parameter: "<< it->name <<" which is unknown to this parser!"; + yCWarning(CouplingICubEyeParamsCOMPONENT) << "User asking for parameter: "<< it->name <<" which is unknown to this parser!"; } } @@ -292,11 +292,11 @@ bool CouplingICubEyeMk3_ParamsParser::parseParams(const yarp::os::Searchabl } -std::string CouplingICubEyeMk3_ParamsParser::getDocumentationOfDeviceParams() const +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: CouplingICubEyeMk3\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"); @@ -308,8 +308,8 @@ std::string CouplingICubEyeMk3_ParamsParser::getDocumentationOfDeviceParams 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 couplingICubEyeMk3 --jointNames --LIMITS::jntPosMin --LIMITS::jntPosMax --COUPLING::device --COUPLING::actuatedAxesNames --COUPLING::actuatedAxesPosMin --COUPLING::actuatedAxesPosMax \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 couplingICubEyeMk3 --jointNames --LIMITS::jntPosMin --LIMITS::jntPosMax --COUPLING::device --COUPLING::actuatedAxesNames --COUPLING::actuatedAxesPosMin --COUPLING::actuatedAxesPosMax \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/couplingICubEyeMk3/CouplingICubEyeMk3_ParamsParser.h b/src/libraries/icubmod/couplingICubEye/CouplingICubEye_ParamsParser.h similarity index 78% rename from src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_ParamsParser.h rename to src/libraries/icubmod/couplingICubEye/CouplingICubEye_ParamsParser.h index 3e9b27561e..1bb6bc0b1a 100644 --- a/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_ParamsParser.h +++ b/src/libraries/icubmod/couplingICubEye/CouplingICubEye_ParamsParser.h @@ -8,11 +8,11 @@ // 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: Mon Dec 2 15:29:09 2024 +// Generated on: Wed Dec 4 09:08:59 2024 -#ifndef COUPLINGICUBEYEMK3_PARAMSPARSER_H -#define COUPLINGICUBEYEMK3_PARAMSPARSER_H +#ifndef COUPLINGICUBEYE_PARAMSPARSER_H +#define COUPLINGICUBEYE_PARAMSPARSER_H #include #include @@ -20,7 +20,7 @@ #include /** -* This class is the parameters parser for class CouplingICubEyeMk3. +* 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 | @@ -35,24 +35,24 @@ * * The device can be launched by yarpdev using one of the following examples (with and without all optional parameters): * \code{.unparsed} -* yarpdev --device couplingICubEyeMk3 --jointNames --LIMITS::jntPosMin --LIMITS::jntPosMax --COUPLING::device --COUPLING::actuatedAxesNames --COUPLING::actuatedAxesPosMin --COUPLING::actuatedAxesPosMax +* yarpdev --device couplingICubEye --jointNames --LIMITS::jntPosMin --LIMITS::jntPosMax --COUPLING::device --COUPLING::actuatedAxesNames --COUPLING::actuatedAxesPosMin --COUPLING::actuatedAxesPosMax * \endcode * * \code{.unparsed} -* yarpdev --device couplingICubEyeMk3 --jointNames --LIMITS::jntPosMin --LIMITS::jntPosMax --COUPLING::device --COUPLING::actuatedAxesNames --COUPLING::actuatedAxesPosMin --COUPLING::actuatedAxesPosMax +* yarpdev --device couplingICubEye --jointNames --LIMITS::jntPosMin --LIMITS::jntPosMax --COUPLING::device --COUPLING::actuatedAxesNames --COUPLING::actuatedAxesPosMin --COUPLING::actuatedAxesPosMax * \endcode * */ -class CouplingICubEyeMk3_ParamsParser : public yarp::dev::IDeviceDriverParams +class CouplingICubEye_ParamsParser : public yarp::dev::IDeviceDriverParams { public: - CouplingICubEyeMk3_ParamsParser(); - ~CouplingICubEyeMk3_ParamsParser() override = default; + CouplingICubEye_ParamsParser(); + ~CouplingICubEye_ParamsParser() override = default; public: - const std::string m_device_classname = {"CouplingICubEyeMk3"}; - const std::string m_device_name = {"couplingICubEyeMk3"}; + const std::string m_device_classname = {"CouplingICubEye"}; + const std::string m_device_name = {"couplingICubEye"}; bool m_parser_is_strict = false; struct parser_version_type { diff --git a/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_params.md b/src/libraries/icubmod/couplingICubEye/CouplingICubEye_params.md similarity index 100% rename from src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3_params.md rename to src/libraries/icubmod/couplingICubEye/CouplingICubEye_params.md diff --git a/src/libraries/icubmod/couplingICubEyeMk3/README.md b/src/libraries/icubmod/couplingICubEye/README.md similarity index 90% rename from src/libraries/icubmod/couplingICubEyeMk3/README.md rename to src/libraries/icubmod/couplingICubEye/README.md index 05cbbce798..f5384f977d 100644 --- a/src/libraries/icubmod/couplingICubEyeMk3/README.md +++ b/src/libraries/icubmod/couplingICubEye/README.md @@ -1,8 +1,8 @@ -![YARP logo](https://raw.githubusercontent.com/robotology/yarp/master/doc/images/yarp-robot-24.png "couplingICubEyeMk3") -## couplingICubEyeMk3 +![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 mk3](https://icub-tech-iit.github.io/documentation/icub_kinematics/icub-vergence-version/icub-vergence-version/) +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 @@ -13,7 +13,7 @@ See the documentation for more details about each interface. | YARP device name | |:----------------:| -| `couplingICubEyeMk3` | +| `couplingICubEye` | Parameters used by this device are: @@ -32,11 +32,11 @@ Parameters used by this device are: Configuration file using `.ini` format: ```ini -device couplingICubEyeMk3 +device couplingICubEye jointNames eyes_tilt l_eye_pan_joint r_eye_pan_joint [COUPLING] -device couplingICubEyeMk3 +device couplingICubEye actuatedAxesNames eyes_version eyes_vergence actuatedAxesPosMin -30.0 0.0 actuatedAxesPosMax 30.0 50.0 diff --git a/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.cpp b/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.cpp deleted file mode 100644 index db4dfd617b..0000000000 --- a/src/libraries/icubmod/couplingICubEyeMk3/CouplingICubEyeMk3.cpp +++ /dev/null @@ -1,158 +0,0 @@ -/* - * 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 "CouplingICubEyeMk3.h" -#include -#include -#include -#include - - -YARP_LOG_COMPONENT(COUPLINGICUBEYEMK3, "yarp.device.couplingICubEyeMk3") - - -bool CouplingICubEyeMk3::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 CouplingICubEyeMk3::open(yarp::os::Searchable& config) { - - if(!parseParams(config)) { - yCError(COUPLINGICUBEYEMK3) << "Error parsing parameters"; - return false; - } - - yCDebug(COUPLINGICUBEYEMK3) << "Opening couplingICubEyeMk3" << config.toString(); - return true; -} - -bool CouplingICubEyeMk3::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(COUPLINGICUBEYEMK3) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size"; - return false; - } - - //version - actAxesPos[1] = (physJointsPos[1] + physJointsPos[2])/2; - //vergence - actAxesPos[2] = (physJointsPos[1] - physJointsPos[2]); - - return true; -} - -bool CouplingICubEyeMk3::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(COUPLINGICUBEYEMK3) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size"; - return false; - } - - //version - actAxesPos[1] = (physJointsPos[1] + physJointsPos[2])/2; - //vergence - actAxesPos[2] = (physJointsPos[1] - physJointsPos[2]); - - return true; -} - -bool CouplingICubEyeMk3::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(COUPLINGICUBEYEMK3) << "convertFromPhysicalJointsToActuatedAxesAcc: input or output vectors have wrong size"; - return false; - } - - //version - actAxesPos[1] = (physJointsPos[1] + physJointsPos[2])/2; - //vergence - actAxesPos[2] = (physJointsPos[1] - physJointsPos[2]); - - return true; -} - -bool CouplingICubEyeMk3::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) { - return false; -} - -bool CouplingICubEyeMk3::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(COUPLINGICUBEYEMK3) << "convertFromActuatedAxesToPhysicalJointsPos: input or output vectors have wrong size"; - return false; - } - - // 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 CouplingICubEyeMk3::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(COUPLINGICUBEYEMK3) << "convertFromPhysicalJointsToActuatedAxesVel: input or output vectors have wrong size"; - return false; - } - - // 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 CouplingICubEyeMk3::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(COUPLINGICUBEYEMK3) << "convertFromPhysicalJointsToActuatedAxesAcc: input or output vectors have wrong size"; - return false; - } - - // 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 CouplingICubEyeMk3::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) { - return false; -} \ No newline at end of file From 0b22e09189a7704db185eb7370cb28f8a2fadae3 Mon Sep 17 00:00:00 2001 From: Martina Gloria Date: Wed, 18 Dec 2024 11:50:14 +0100 Subject: [PATCH 3/5] Fix bug --- src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp | 5 +++++ src/libraries/icubmod/couplingICubEye/README.md | 6 +++--- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp index d901434376..95560cedb4 100644 --- a/src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp +++ b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp @@ -37,6 +37,11 @@ bool CouplingICubEye::open(yarp::os::Searchable& config) { return false; } + if(!populateCouplingParameters) { + yCError(COUPLINGICUBEYE) << "Error populating coupling parameters"; + return false; + } + yCDebug(COUPLINGICUBEYE) << "Opening couplingICubEye" << config.toString(); return true; } diff --git a/src/libraries/icubmod/couplingICubEye/README.md b/src/libraries/icubmod/couplingICubEye/README.md index f5384f977d..dc28dcb484 100644 --- a/src/libraries/icubmod/couplingICubEye/README.md +++ b/src/libraries/icubmod/couplingICubEye/README.md @@ -37,9 +37,9 @@ jointNames eyes_tilt l_eye_pan_joint r_eye_pan_joint [COUPLING] device couplingICubEye -actuatedAxesNames eyes_version eyes_vergence -actuatedAxesPosMin -30.0 0.0 -actuatedAxesPosMax 30.0 50.0 +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 From 88d8e4a4205debe13dbc8f986e76fc04a149d61c Mon Sep 17 00:00:00 2001 From: Martina Gloria <114698424+martinaxgloria@users.noreply.github.com> Date: Wed, 18 Dec 2024 11:57:23 +0100 Subject: [PATCH 4/5] Update CouplingICubEye.cpp --- src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp index 95560cedb4..9cf5823892 100644 --- a/src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp +++ b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.cpp @@ -37,7 +37,7 @@ bool CouplingICubEye::open(yarp::os::Searchable& config) { return false; } - if(!populateCouplingParameters) { + if(!populateCouplingParameters()) { yCError(COUPLINGICUBEYE) << "Error populating coupling parameters"; return false; } @@ -171,4 +171,4 @@ bool CouplingICubEye::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig bool CouplingICubEye::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) { return false; -} \ No newline at end of file +} From cca651eda3ccec020bbf4c0c0b17bc25771363ad Mon Sep 17 00:00:00 2001 From: Martina Gloria <114698424+martinaxgloria@users.noreply.github.com> Date: Wed, 18 Dec 2024 15:03:23 +0100 Subject: [PATCH 5/5] Update CouplingICubEye.h --- src/libraries/icubmod/couplingICubEye/CouplingICubEye.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/libraries/icubmod/couplingICubEye/CouplingICubEye.h b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.h index e433d7a28d..965275c9d4 100644 --- a/src/libraries/icubmod/couplingICubEye/CouplingICubEye.h +++ b/src/libraries/icubmod/couplingICubEye/CouplingICubEye.h @@ -24,8 +24,7 @@ YARP_DECLARE_LOG_COMPONENT(COUPLINGICUBEYE) /** * Coupling law from https://icub-tech-iit.github.io/documentation/icub_kinematics/icub-vergence-version/icub-vergence-version/ */ -/** TBD - */ + class CouplingICubEye : public yarp::dev::DeviceDriver, public yarp::dev::ImplementJointCoupling, public CouplingICubEye_ParamsParser { @@ -53,4 +52,4 @@ class CouplingICubEye : public yarp::dev::DeviceDriver, bool populateCouplingParameters(); }; -#endif // COUPLINGICUBEYE_H \ No newline at end of file +#endif // COUPLINGICUBEYE_H