diff --git a/libraries/YarpPlugins/KdlSolver/CMakeLists.txt b/libraries/YarpPlugins/KdlSolver/CMakeLists.txt index 5fd9812ca..0320ad53e 100644 --- a/libraries/YarpPlugins/KdlSolver/CMakeLists.txt +++ b/libraries/YarpPlugins/KdlSolver/CMakeLists.txt @@ -4,10 +4,11 @@ endif() yarp_prepare_plugin(KdlSolver CATEGORY device - TYPE roboticslab::KdlSolver + TYPE KdlSolver INCLUDE KdlSolver.hpp DEFAULT ON - DEPENDS "ENABLE_ScrewTheoryLib;ENABLE_KdlVectorConverterLib;orocos_kdl_FOUND") + DEPENDS "ENABLE_ScrewTheoryLib;ENABLE_KdlVectorConverterLib;orocos_kdl_FOUND" + GENERATE_PARSER) if(NOT SKIP_KdlSolver) @@ -23,6 +24,11 @@ if(NOT SKIP_KdlSolver) LogComponent.hpp LogComponent.cpp) + if(YARP_VERSION VERSION_GREATER_EQUAL 3.10) + target_sources(KdlSolver PRIVATE KdlSolver_ParamsParser.h + KdlSolver_ParamsParser.cpp) + endif() + target_link_libraries(KdlSolver YARP::YARP_os YARP::YARP_dev ${orocos_kdl_LIBRARIES} diff --git a/libraries/YarpPlugins/KdlSolver/DeviceDriverImpl.cpp b/libraries/YarpPlugins/KdlSolver/DeviceDriverImpl.cpp index a21983f4d..722c89ee4 100644 --- a/libraries/YarpPlugins/KdlSolver/DeviceDriverImpl.cpp +++ b/libraries/YarpPlugins/KdlSolver/DeviceDriverImpl.cpp @@ -147,6 +147,12 @@ namespace bool KdlSolver::open(yarp::os::Searchable & config) { + if (!parseParams(config)) + { + yCError(KDLS) << "Failed to parse parameters"; + return false; + } + isQuiet = config.check("quiet", "disable logging"); //-- kinematics diff --git a/libraries/YarpPlugins/KdlSolver/KdlSolver.hpp b/libraries/YarpPlugins/KdlSolver/KdlSolver.hpp index d28e3e5df..358234453 100644 --- a/libraries/YarpPlugins/KdlSolver/KdlSolver.hpp +++ b/libraries/YarpPlugins/KdlSolver/KdlSolver.hpp @@ -5,6 +5,7 @@ #include +#include #include #include @@ -14,15 +15,15 @@ #include "ICartesianSolver.h" #include "LogComponent.hpp" - -namespace roboticslab -{ +#if YARP_VERSION_COMPARE(>=, 3, 10, 0) +#include "KdlSolver_ParamsParser.h" +#endif /** * @ingroup YarpPlugins * @defgroup KdlSolver * - * @brief Contains roboticslab::KdlSolver. + * @brief Contains KdlSolver. */ /** @@ -30,10 +31,13 @@ namespace roboticslab * @brief The KdlSolver class implements ICartesianSolver. */ class KdlSolver : public yarp::dev::DeviceDriver, - public ICartesianSolver +#if YARP_VERSION_COMPARE(>=, 3, 10, 0) + public KdlSolver_ParamsParser, +#endif + public roboticslab::ICartesianSolver { public: - // -- ICartesianSolver declarations. Implementation in ICartesianSolverImpl.cpp-- + // -- ICartesianSolver declarations. Implementation in ICartesianSolverImpl.cpp -- // Get number of joints for which the solver has been configured. int getNumJoints() override; @@ -48,9 +52,7 @@ class KdlSolver : public yarp::dev::DeviceDriver, bool restoreOriginalChain() override; // Change reference frame. - bool changeOrigin(const std::vector &x_old_obj, - const std::vector &x_new_old, - std::vector &x_new_obj) override; + bool changeOrigin(const std::vector &x_old_obj, const std::vector &x_new_old, std::vector &x_new_obj) override; // Perform forward kinematics. bool fwdKin(const std::vector &q, std::vector &x) override; @@ -59,12 +61,10 @@ class KdlSolver : public yarp::dev::DeviceDriver, bool poseDiff(const std::vector &xLhs, const std::vector &xRhs, std::vector &xOut) override; // Perform inverse kinematics. - bool invKin(const std::vector &xd, const std::vector &qGuess, std::vector &q, - reference_frame frame) override; + bool invKin(const std::vector &xd, const std::vector &qGuess, std::vector &q, reference_frame frame) override; // Perform differential inverse kinematics. - bool diffInvKin(const std::vector &q, const std::vector &xdot, std::vector &qdot, - reference_frame frame) override; + bool diffInvKin(const std::vector &q, const std::vector &xdot, std::vector &qdot, reference_frame frame) override; // Perform inverse dynamics. bool invDyn(const std::vector &q, std::vector &t) override; @@ -84,10 +84,7 @@ class KdlSolver : public yarp::dev::DeviceDriver, mutable std::mutex mtx; - /** The chain. **/ KDL::Chain chain; - - /** To store a copy of the original chain. **/ KDL::Chain originalChain; KDL::ChainFkSolverPos * fkSolverPos {nullptr}; @@ -98,6 +95,4 @@ class KdlSolver : public yarp::dev::DeviceDriver, bool isQuiet; }; -} // namespace roboticslab - #endif // __KDL_SOLVER_HPP__ diff --git a/libraries/YarpPlugins/KdlSolver/KdlSolver_ParamsParser.cpp b/libraries/YarpPlugins/KdlSolver/KdlSolver_ParamsParser.cpp new file mode 100644 index 000000000..75e625c71 --- /dev/null +++ b/libraries/YarpPlugins/KdlSolver/KdlSolver_ParamsParser.cpp @@ -0,0 +1,347 @@ +/* + * 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: Sat Nov 16 21:25:22 2024 + + +#include "KdlSolver_ParamsParser.h" +#include +#include + +namespace { + YARP_LOG_COMPONENT(KdlSolverParamsCOMPONENT, "yarp.device.KdlSolver") +} + + +KdlSolver_ParamsParser::KdlSolver_ParamsParser() +{ + //Default value of parametergravity + { + m_gravity.clear(); + yarp::os::Value tempVal; + tempVal.fromString(m_gravity_defaultValue.c_str()); + yarp::os::Bottle* tempBot = tempVal.asList(); + if (tempBot && tempBot->size()!=0) + { + for (size_t i=0; isize(); i++) + { + m_gravity.push_back(tempBot->get(i).asFloat64()); + } + } + else + { + yError() <<"parameter 'gravity' is not a properly formatted bottle"; + } + } + + //Default value of parameterweights + { + m_weights.clear(); + yarp::os::Value tempVal; + tempVal.fromString(m_weights_defaultValue.c_str()); + yarp::os::Bottle* tempBot = tempVal.asList(); + if (tempBot && tempBot->size()!=0) + { + for (size_t i=0; isize(); i++) + { + m_weights.push_back(tempBot->get(i).asFloat64()); + } + } + else + { + yError() <<"parameter 'weights' is not a properly formatted bottle"; + } + } + +} + + +std::vector KdlSolver_ParamsParser::getListOfParams() const +{ + std::vector params; + params.push_back("quiet"); + params.push_back("kinematics"); + params.push_back("gravity"); + params.push_back("ikPos"); + params.push_back("ikVel"); + params.push_back("epsPos"); + params.push_back("maxIterPos"); + params.push_back("epsVel"); + params.push_back("maxIterVel"); + params.push_back("lambda"); + params.push_back("weights"); + params.push_back("invKinStrategy"); + return params; +} + + +bool KdlSolver_ParamsParser::parseParams(const yarp::os::Searchable & config) +{ + //Check for --help option + if (config.check("help")) + { + yCInfo(KdlSolverParamsCOMPONENT) << getDocumentationOfDeviceParams(); + } + + std::string config_string = config.toString(); + yarp::os::Property prop_check(config_string.c_str()); + //Parser of parameter quiet + { + if (config.check("quiet")) + { + m_quiet = config.find("quiet").asBool(); + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'quiet' using value:" << m_quiet; + } + else + { + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'quiet' using DEFAULT value:" << m_quiet; + } + prop_check.unput("quiet"); + } + + //Parser of parameter kinematics + { + if (config.check("kinematics")) + { + m_kinematics = config.find("kinematics").asString(); + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'kinematics' using value:" << m_kinematics; + } + else + { + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'kinematics' using DEFAULT value:" << m_kinematics; + } + prop_check.unput("kinematics"); + } + + //Parser of parameter gravity + { + if (config.check("gravity")) + { + { + m_gravity.clear(); + yarp::os::Bottle* tempBot = config.find("gravity").asList(); + if (tempBot) + { + std::string tempBots = tempBot->toString(); + for (size_t i=0; isize(); i++) + { + m_gravity.push_back(tempBot->get(i).asFloat64()); + } + } + else + { + yCError(KdlSolverParamsCOMPONENT) <<"parameter 'gravity' is not a properly formatted bottle"; + } + } + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'gravity' using value:" << m_gravity; + } + else + { + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'gravity' using DEFAULT value:" << m_gravity; + } + prop_check.unput("gravity"); + } + + //Parser of parameter ikPos + { + if (config.check("ikPos")) + { + m_ikPos = config.find("ikPos").asString(); + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'ikPos' using value:" << m_ikPos; + } + else + { + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'ikPos' using DEFAULT value:" << m_ikPos; + } + prop_check.unput("ikPos"); + } + + //Parser of parameter ikVel + { + if (config.check("ikVel")) + { + m_ikVel = config.find("ikVel").asString(); + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'ikVel' using value:" << m_ikVel; + } + else + { + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'ikVel' using DEFAULT value:" << m_ikVel; + } + prop_check.unput("ikVel"); + } + + //Parser of parameter epsPos + { + if (config.check("epsPos")) + { + m_epsPos = config.find("epsPos").asFloat64(); + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'epsPos' using value:" << m_epsPos; + } + else + { + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'epsPos' using DEFAULT value:" << m_epsPos; + } + prop_check.unput("epsPos"); + } + + //Parser of parameter maxIterPos + { + if (config.check("maxIterPos")) + { + m_maxIterPos = config.find("maxIterPos").asInt64(); + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'maxIterPos' using value:" << m_maxIterPos; + } + else + { + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'maxIterPos' using DEFAULT value:" << m_maxIterPos; + } + prop_check.unput("maxIterPos"); + } + + //Parser of parameter epsVel + { + if (config.check("epsVel")) + { + m_epsVel = config.find("epsVel").asFloat64(); + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'epsVel' using value:" << m_epsVel; + } + else + { + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'epsVel' using DEFAULT value:" << m_epsVel; + } + prop_check.unput("epsVel"); + } + + //Parser of parameter maxIterVel + { + if (config.check("maxIterVel")) + { + m_maxIterVel = config.find("maxIterVel").asInt64(); + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'maxIterVel' using value:" << m_maxIterVel; + } + else + { + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'maxIterVel' using DEFAULT value:" << m_maxIterVel; + } + prop_check.unput("maxIterVel"); + } + + //Parser of parameter lambda + { + if (config.check("lambda")) + { + m_lambda = config.find("lambda").asFloat64(); + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'lambda' using value:" << m_lambda; + } + else + { + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'lambda' using DEFAULT value:" << m_lambda; + } + prop_check.unput("lambda"); + } + + //Parser of parameter weights + { + if (config.check("weights")) + { + { + m_weights.clear(); + yarp::os::Bottle* tempBot = config.find("weights").asList(); + if (tempBot) + { + std::string tempBots = tempBot->toString(); + for (size_t i=0; isize(); i++) + { + m_weights.push_back(tempBot->get(i).asFloat64()); + } + } + else + { + yCError(KdlSolverParamsCOMPONENT) <<"parameter 'weights' is not a properly formatted bottle"; + } + } + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'weights' using value:" << m_weights; + } + else + { + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'weights' using DEFAULT value:" << m_weights; + } + prop_check.unput("weights"); + } + + //Parser of parameter invKinStrategy + { + if (config.check("invKinStrategy")) + { + m_invKinStrategy = config.find("invKinStrategy").asString(); + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'invKinStrategy' using value:" << m_invKinStrategy; + } + else + { + yCInfo(KdlSolverParamsCOMPONENT) << "Parameter 'invKinStrategy' using DEFAULT value:" << m_invKinStrategy; + } + prop_check.unput("invKinStrategy"); + } + + /* + //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(KdlSolverParamsCOMPONENT) << "User asking for parameter: "<name <<" which is unknown to this parser!"; + extra_params_found = true; + } + else + { + yCWarning(KdlSolverParamsCOMPONENT) << "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 KdlSolver_ParamsParser::getDocumentationOfDeviceParams() const +{ + std::string doc; + doc = doc + std::string("\n=============================================\n"); + doc = doc + std::string("This is the help for device: KdlSolver\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("'quiet': disable logging\n"); + doc = doc + std::string("'kinematics': path to file with description of robot kinematics\n"); + doc = doc + std::string("'gravity': gravity vector\n"); + doc = doc + std::string("'ikPos': IK position solver algorithm (lma, nrjl, st, id)\n"); + doc = doc + std::string("'ikVel': IK velocity solver algorithm (pinv, wdls)\n"); + doc = doc + std::string("'epsPos': IK position solver precision\n"); + doc = doc + std::string("'maxIterPos': IK position solver max iterations\n"); + doc = doc + std::string("'epsVel': IK velocity solver precision\n"); + doc = doc + std::string("'maxIterVel': IK velocity solver max iterations\n"); + doc = doc + std::string("'lambda': lambda parameter for diff IK\n"); + doc = doc + std::string("'weights': LMA algorithm weights\n"); + doc = doc + std::string("'invKinStrategy': IK configuration strategy\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 KdlSolver --quiet false --kinematics none.ini --gravity \" (0.0 0.0 9.81) \" --ikPos st --ikVel pinv --epsPos 1e-5 --maxIterPos 1000 --epsVel 1e-5 --maxIterVel 150 --lambda 0.01 --weights \" (1.0 1.0 1.0 0.1 0.1 0.1) \" --invKinStrategy leastOverallAngularDisplacement\n"; + doc = doc + std::string("Using only mandatory params:\n"); + doc = doc + " yarpdev --device KdlSolver\n"; + doc = doc + std::string("=============================================\n\n"); return doc; +} diff --git a/libraries/YarpPlugins/KdlSolver/KdlSolver_ParamsParser.h b/libraries/YarpPlugins/KdlSolver/KdlSolver_ParamsParser.h new file mode 100644 index 000000000..5e150edb1 --- /dev/null +++ b/libraries/YarpPlugins/KdlSolver/KdlSolver_ParamsParser.h @@ -0,0 +1,102 @@ +/* + * 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: Sat Nov 16 21:25:22 2024 + + +#ifndef KDLSOLVER_PARAMSPARSER_H +#define KDLSOLVER_PARAMSPARSER_H + +#include +#include +#include +#include + +/** +* This class is the parameters parser for class KdlSolver. +* +* These are the used parameters: +* | Group name | Parameter name | Type | Units | Default Value | Required | Description | Notes | +* |:----------:|:--------------:|:--------------:|:-----:|:-------------------------------:|:--------:|:-------------------------------------------------:|:-----:| +* | - | quiet | bool | - | false | 0 | disable logging | test | +* | - | kinematics | string | - | none.ini | 0 | path to file with description of robot kinematics | - | +* | - | gravity | vector | m/s^2 | (0.0 0.0 9.81) | 0 | gravity vector | - | +* | - | ikPos | string | - | st | 0 | IK position solver algorithm (lma, nrjl, st, id) | - | +* | - | ikVel | string | - | pinv | 0 | IK velocity solver algorithm (pinv, wdls) | - | +* | - | epsPos | double | m | 1e-5 | 0 | IK position solver precision | - | +* | - | maxIterPos | int | - | 1000 | 0 | IK position solver max iterations | - | +* | - | epsVel | double | m | 1e-5 | 0 | IK velocity solver precision | - | +* | - | maxIterVel | int | - | 150 | 0 | IK velocity solver max iterations | - | +* | - | lambda | double | - | 0.01 | 0 | lambda parameter for diff IK | - | +* | - | weights | vector | - | (1.0 1.0 1.0 0.1 0.1 0.1) | 0 | LMA algorithm weights | - | +* | - | invKinStrategy | string | - | leastOverallAngularDisplacement | 0 | IK configuration strategy | - | +* +* The device can be launched by yarpdev using one of the following examples (with and without all optional parameters): +* \code{.unparsed} +* yarpdev --device KdlSolver --quiet false --kinematics none.ini --gravity \" (0.0 0.0 9.81) \" --ikPos st --ikVel pinv --epsPos 1e-5 --maxIterPos 1000 --epsVel 1e-5 --maxIterVel 150 --lambda 0.01 --weights \" (1.0 1.0 1.0 0.1 0.1 0.1) \" --invKinStrategy leastOverallAngularDisplacement +* \endcode +* +* \code{.unparsed} +* yarpdev --device KdlSolver +* \endcode +* +*/ + +class KdlSolver_ParamsParser : public yarp::dev::IDeviceDriverParams +{ +public: + KdlSolver_ParamsParser(); + ~KdlSolver_ParamsParser() override = default; + +public: + const std::string m_device_classname = {"KdlSolver"}; + const std::string m_device_name = {"KdlSolver"}; + 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_quiet_defaultValue = {"false"}; + const std::string m_kinematics_defaultValue = {"none.ini"}; + const std::string m_gravity_defaultValue = {"(0.0 0.0 9.81)"}; + const std::string m_ikPos_defaultValue = {"st"}; + const std::string m_ikVel_defaultValue = {"pinv"}; + const std::string m_epsPos_defaultValue = {"1e-5"}; + const std::string m_maxIterPos_defaultValue = {"1000"}; + const std::string m_epsVel_defaultValue = {"1e-5"}; + const std::string m_maxIterVel_defaultValue = {"150"}; + const std::string m_lambda_defaultValue = {"0.01"}; + const std::string m_weights_defaultValue = {"(1.0 1.0 1.0 0.1 0.1 0.1)"}; + const std::string m_invKinStrategy_defaultValue = {"leastOverallAngularDisplacement"}; + + bool m_quiet = {false}; + std::string m_kinematics = {"none.ini"}; + std::vector m_gravity = { }; //Default values for lists are managed in the class constructor. It is highly recommended to provide a suggested value also for optional string parameters. + std::string m_ikPos = {"st"}; + std::string m_ikVel = {"pinv"}; + double m_epsPos = {1e-5}; + int m_maxIterPos = {1000}; + double m_epsVel = {1e-5}; + int m_maxIterVel = {150}; + double m_lambda = {0.01}; + std::vector m_weights = { }; //Default values for lists are managed in the class constructor. It is highly recommended to provide a suggested value also for optional string parameters. + std::string m_invKinStrategy = {"leastOverallAngularDisplacement"}; + + 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/libraries/YarpPlugins/KdlSolver/KdlSolver_params.md b/libraries/YarpPlugins/KdlSolver/KdlSolver_params.md new file mode 100644 index 000000000..a12204cc7 --- /dev/null +++ b/libraries/YarpPlugins/KdlSolver/KdlSolver_params.md @@ -0,0 +1,14 @@ +| Group | Parameter | Type | Units | Default Value | Required | Description | Notes | +|:-----:|:--------------:|:--------------:|:-----:|:-------------------------------:|:--------:|:-------------------------------------------------:|:-----:| +| | quiet | bool | | false | no | disable logging | test | +| | kinematics | string | | none.ini | no | path to file with description of robot kinematics | | +| | gravity | vector | m/s^2 | (0.0 0.0 9.81) | no | gravity vector | | +| | ikPos | string | | st | no | IK position solver algorithm (lma, nrjl, st, id) | | +| | ikVel | string | | pinv | no | IK velocity solver algorithm (pinv, wdls) | | +| | epsPos | double | m | 1e-5 | no | IK position solver precision | | +| | maxIterPos | int | | 1000 | no | IK position solver max iterations | | +| | epsVel | double | m | 1e-5 | no | IK velocity solver precision | | +| | maxIterVel | int | | 150 | no | IK velocity solver max iterations | | +| | lambda | double | | 0.01 | no | lambda parameter for diff IK | | +| | weights | vector | | (1.0 1.0 1.0 0.1 0.1 0.1) | no | LMA algorithm weights | | +| | invKinStrategy | string | | leastOverallAngularDisplacement | no | IK configuration strategy | |